一起做RGB-D SLAM (2)
roswiki
roswiki 1768 0
2016-08-10 20:06


第二讲 从图像到点云



本讲中,我们将带领读者,编写一个将图像转换为点云的程序。该程序是后期处理地图的基础。最简单的点云地图即是把不同位置的点云进行拼接得到的。
当我们使用RGB-D相机时,会从相机里读到两种数据:彩色图像和深度图像。如果你有Kinect和ros,可以运行:

roslaunch openni_launch openni.launch


使Kinect工作。随后,如果PC连接上了Kinect,彩色图像与深度图像就会发布在 /camera/rgb/image_color 和 /camera/depth_registered/image_raw 中。你可以通过:

rosrun image_view image_view image:=/camera/rgb/image_color


来显示彩色图像。或者,你也可以在Rviz里看到图像与点云的可视化数据。
小萝卜:可是师兄!我现在手边没有Kinect,该怎么办啊!
师兄:没关系!你可以下载我们给你提供的数据。实际上就是下面两张图片啦!

小萝卜:怎么深度图是一团黑的呀!
师兄:请睁大眼睛仔细看!怎么可能是黑的!
小萝卜:呃……可是确实是黑的啊!
师兄:对!这是由于画面里的物体离我们比较近,所以看上去比较黑。但是你实际去读的话可是有数据的哦!
重要的备注:
  1. 这两张图来自于nyuv2数据集:http://cs.nyu.edu/~silberman/datasets/ 原图格式是ppm和pgm的,被我转成了png格式(否则博客园不让传……)。
  2. 你可以直接另存为这两个图,也可以到我的git里面获取这两个图。
  3. 实际Kinect里(或其他rgb-d相机里)直接采到的RGB图和深度图可能会有些小问题:
    • 有一些时差(约几到十几个毫秒)。这个时差的存在,会产生“RGB图已经向右转了,怎么深度图还没转”的感觉哦。
    • 光圈中心未对齐。因为深度毕竟是靠另一个相机获取的,所以深度传感器和彩色传感器参数可能不一致。
    • 深度图里有很多“洞”。因为RGB-D相机不是万能的,它有一个探测距离的限制啦!太远或太近的东西都是看不见的呢。关于这些“洞”,我们暂时睁一只眼闭一只眼,不去理它。以后我们也可以靠双边bayes滤波器去填这些洞。但是!这是RGB-D相机本身的局限性。软件算法顶多给它修修补补,并不能完全弥补它的缺陷。

不过请你放心,在我们给出的这两个图中,都进行了预处理。你可以认为“深度图就是彩色图里每个像素距传感器的距离”啦!
师兄:现在,我们要把这两个图转成点云啦,因为计算每个像素的空间点位置,可是后面配准、拼图等一系列事情的基础呢。比如,在配准时,必须知道特征点的3D位置呢,这时候就要用到我们这里讲到的知识啦!
小萝卜:听起来很重要的样子!
师兄:对!所以请读者朋友务必掌握好这部分的内容啦!


从2D到3D(数学部分)


上面两个图像给出了机器人外部世界的一个局部的信息。假设这个世界由一个点云来描述:<span tabindex="0" class="MathJax" id="MathJax-Element-1-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="X={x1,&#x2026;,xn}">X={x 1 ,,x n } X={x1,…,xn} . 其中每一个点呢,有 <span tabindex="0" class="MathJax" id="MathJax-Element-2-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="r,g,b,x,y,z">r,g,b,x,y,z r,g,b,x,y,z 一共6个分量,表示它们的颜色与空间位置。颜色方面,主要由彩色图像记录; 而空间位置,可以由图像和相机模型、姿态一起计算出来。
对于常规相机,SLAM里使用针孔相机模型(图来自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf ):


简而言之,一个空间点<span tabindex="0" class="MathJax" id="MathJax-Element-3-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="[x,y,z]">[x,y,z] [x,y,z] 和它在图像中的像素坐标<span tabindex="0" class="MathJax" id="MathJax-Element-4-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="[u,v,d]">[u,v,d] [u,v,d] (<span tabindex="0" class="MathJax" id="MathJax-Element-5-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="d">d d 指深度数据) 的对应关系是这样的:
<span tabindex="0" class="MathJax" id="MathJax-Element-6-Frame" role="presentation" style="position:relative;" data-mathml="u=x&#x22C5;fxz+cx">u=xf x z +c x  u=x⋅fxz+cx


<span tabindex="0" class="MathJax" id="MathJax-Element-7-Frame" role="presentation" style="position:relative;" data-mathml="v=y&#x22C5;fyz+cy">v=yf y z +c y v=y⋅fyz+cy


<span tabindex="0" class="MathJax" id="MathJax-Element-8-Frame" role="presentation" style="position:relative;" data-mathml="d=z&#x22C5;s">d=zs d=z⋅s



其中,<span tabindex="0" class="MathJax" id="MathJax-Element-9-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="fx,fy">f x ,f y fx,fy 指相机在<span tabindex="0" class="MathJax" id="MathJax-Element-10-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="x,y">x,y x,y 两个轴上的焦距,<span tabindex="0" class="MathJax" id="MathJax-Element-11-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="cx,cy">c x ,c y cx,cy 指相机的光圈中心,<span tabindex="0" class="MathJax" id="MathJax-Element-12-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="s">s s 指深度图的缩放因子。
小萝卜:好晕啊!突然冒出这么多个变量!
师兄:别急啊,这已经是很简单的模型了,等你熟悉了就不觉得复杂了。
这个公式是从<span tabindex="0" class="MathJax" id="MathJax-Element-13-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="(x,y,z)">(x,y,z) (x,y,z) 推到<span tabindex="0" class="MathJax" id="MathJax-Element-14-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="(u,v,d)">(u,v,d) (u,v,d) 的。反之,我们也可以把它写成已知<span tabindex="0" class="MathJax" id="MathJax-Element-15-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="(u,v,d)">(u,v,d) (u,v,d) ,推导<span tabindex="0" class="MathJax" id="MathJax-Element-16-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="(x,y,z)">(x,y,z) (x,y,z) 的方式。请读者自己推导一下。
不,还是我们来推导吧……公式是这样的:
<span tabindex="0" class="MathJax" id="MathJax-Element-17-Frame" role="presentation" style="position:relative;" data-mathml="z=d/s">z=d/s z=d/s


<span tabindex="0" class="MathJax" id="MathJax-Element-18-Frame" role="presentation" style="position:relative;" data-mathml="x=(u&#x2212;cx)&#x22C5;z/fx">x=(uc x )z/f x  x=(u−cx)⋅z/fx


<span tabindex="0" class="MathJax" id="MathJax-Element-19-Frame" role="presentation" style="position:relative;" data-mathml="y=(v&#x2212;cy)&#x22C5;z/fy">y=(vc y )z/f y y=(v−cy)⋅z/fy



怎么样,是不是很简单呢?事实上根据这个公式就可以构建点云啦。
通常,我们会把<span tabindex="0" class="MathJax" id="MathJax-Element-20-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="fx,fy,cx,cy">f x ,f y ,c x ,c y fx,fy,cx,cy 这四个参数定义为相机的内参矩阵<span tabindex="0" class="MathJax" id="MathJax-Element-21-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="C">C C ,也就是相机做好之后就不会变的参数。相机的内参可以用很多方法来标定,详细的步骤比较繁琐,我们这里就不提了。给定内参之后呢,每个点的空间位置与像素坐标就可以用简单的矩阵模型来描述了:
<span tabindex="0" class="MathJax" id="MathJax-Element-22-Frame" role="presentation" style="position:relative;" data-mathml="s&#x22C5;[uv1]=C&#x22C5;(R&#x22C5;[xyz]+t)">suv1 =CRxyz +t s⋅[uv1]=C⋅(R⋅[xyz]+t)



其中,<span tabindex="0" class="MathJax" id="MathJax-Element-23-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="R">R R 和<span tabindex="0" class="MathJax" id="MathJax-Element-24-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="t">t t 是相机的姿态。<span tabindex="0" class="MathJax" id="MathJax-Element-25-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="R">R R 代表旋转矩阵,<span tabindex="0" class="MathJax" id="MathJax-Element-26-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="t">t t 代表位移矢量。因为我们现在做的是单幅点云,认为相机没有旋转和平移。所以,把<span tabindex="0" class="MathJax" id="MathJax-Element-27-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="R">R R 设成单位矩阵<span tabindex="0" class="MathJax" id="MathJax-Element-28-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="I">I I ,把<span tabindex="0" class="MathJax" id="MathJax-Element-29-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="t">t t 设成了零。<span tabindex="0" class="MathJax" id="MathJax-Element-30-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="s">s s 是scaling factor,即深度图里给的数据与实际距离的比例。由于深度图给的都是short (mm单位),<span tabindex="0" class="MathJax" id="MathJax-Element-31-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="s">s s 通常为1000。
小萝卜:于是就有了上面那个<span tabindex="0" class="MathJax" id="MathJax-Element-32-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="(u,v,d)">(u,v,d) (u,v,d) 转<span tabindex="0" class="MathJax" id="MathJax-Element-33-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="(x,y,z)">(x,y,z) (x,y,z) 的公式?
师兄:对!真聪明!如果相机发生了位移和旋转,那么只要对这些点进行位移和旋转操作即可。


从2D到3D (编程部分)


下面,我们来实现一个程序,完成从图像到点云的转换。请在上一节讲到的 代码根目录/src/ 文件夹中新建一个generatePointCloud.cpp文件:

touch src/generatePointCloud.cpp


小萝卜:师兄!一个工程里可以有好几个main函数么?
师兄:对呀,cmake允许你自己定义编译的过程。我们会把这个cpp也编译成一个可执行的二进制,只要在cmakelists.txt里作相应的更改便行了。
接下来,请在刚建的文件里输入下面的代码。为保证行文的连贯性,我们先给出完整的代码,然后在重要的地方加以解释。建议新手逐字自己敲一遍,你会掌握得更牢固。




// C++ 标准库
#include &lt;iostream&gt;
#include &lt;string&gt;
using namespace std;
  
// OpenCV 库
#include &lt;opencv2/core/core.hpp&gt;
#include &lt;opencv2/highgui/highgui.hpp&gt;
  
// PCL 库
#include &lt;pcl/io/pcd_io.h&gt;
#include &lt;pcl/point_types.h&gt;
  
// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud&lt;PointT&gt; PointCloud; 
  
// 相机内参
const double camera_factor = 1000;
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;
  
// 主函数 
int main( int argc, char** argv )
{
    // 读取./data/rgb.png和./data/depth.png,并转化为点云
  
    // 图像矩阵
    cv::Mat rgb, depth;
    // 使用cv::imread()来读取图像
    // API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
    rgb = cv::imread( "./data/rgb.png" );
    // rgb 图像是8UC3的彩色图像
    // depth 是16UC1的单通道图像,注意flags设置-1,表示读取原始数据不做任何修改
    depth = cv::imread( "./data/depth.png", -1 );
  
    // 点云变量
    // 使用智能指针,创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud ( new PointCloud );
    // 遍历深度图
    for (int m = 0; m &lt; depth.rows; m++)
        for (int n=0; n &lt; depth.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = depth.ptr&lt;ushort&gt;(m)[n];
            // d 可能没有值,若如此,跳过此点
            if (d == 0)
                continue;
            // d 存在值,则向点云增加一个点
            PointT p;
  
            // 计算这个点的空间坐标
            p.z = double(d) / camera_factor;
            p.x = (n - camera_cx) * p.z / camera_fx;
            p.y = (m - camera_cy) * p.z / camera_fy;
              
            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图,所以按下面的顺序获取颜色
            p.b = rgb.ptr&lt;uchar&gt;(m)[n*3];
            p.g = rgb.ptr&lt;uchar&gt;(m)[n*3+1];
            p.r = rgb.ptr&lt;uchar&gt;(m)[n*3+2];
  
            // 把p加入到点云中
            cloud-&gt;points.push_back( p );
        }
    // 设置并保存点云
    cloud-&gt;height = 1;
    cloud-&gt;width = cloud-&gt;points.size();
    cout&lt;&lt;"point cloud size = "&lt;&lt;cloud-&gt;points.size()&lt;&lt;endl;
    cloud-&gt;is_dense = false;
    pcl::io::savePCDFile( "./data/pointcloud.pcd", *cloud );
    // 清除数据并退出
    cloud-&gt;points.clear();
    cout&lt;&lt;"Point cloud saved."&lt;&lt;endl;
    return 0;
}


程序运行需要数据。请把上面的那两个图存放在 代码根目录/data 下(没有这个文件夹就新建一个)。
我们使用OpenCV的imread函数读取图片。在OpenCV2里,图像是以矩阵(cv::MAt)作为基本的数据结构。Mat结构既可以帮你管理内存、像素信息,还支持一些常见的矩阵运算,是非常方便的结构。彩色图像含有R,G,B三个通道,每个通道占8个bit(也就是unsigned char),故称为8UC3(8位unsigend char, 3通道)结构。而深度图则是单通道的图像,每个像素由16个bit组成(也就是C++里的unsigned short),像素的值代表该点离传感器的距离。通常1000的值代表1米,所以我们把camera_factor设置成1000. 这样,深度图里每个像素点的读数除以1000,就是它离你的真实距离了。
接下来,我们按照“先列后行”的顺序,遍历了整张深度图。在这个双重循环中:

for (int m = 0; m &lt; depth.rows; m++)
     for (int n=0; n &lt; depth.cols; n++)


m指图像的行,n是图像的列。它和空间点的坐标系关系是这样的:

深度图第m行,第n行的数据可以使用depth.ptr<ushort>(m) [n]来获取。其中,cv::Mat的ptr函数会返回指向该图像第m行数据的头指针。然后加上位移n后,这个指针指向的数据就是我们需要读取的数据啦。
计算三维点坐标的公式我们已经给出过了,代码里原封不动地实现了一遍。我们根据这个公式,新增了一个空间点,并放入了点云中。最后,把整个点云存储为 ./data/pointcloud.pcd 文件。


编译并运行


最后,我们在src/CMakeLists.txt里加入几行代码,告诉编译器我们希望编译这个程序。请在此文件中加入以下几行:


# 增加PCL库的依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )
  
# 增加opencv的依赖
FIND_PACKAGE( OpenCV REQUIRED )
  
# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
  
ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp )
TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS} 
    ${PCL_LIBRARIES} )


然后,编译新的工程:

cd build
cmake ..
make
cd ..


如果编译通过,就可在bin目录下找到新写的二进制:generate_pointcloud 运行它:

bin/generate_pointcloud


即可在data目录下生成点云文件。现在,你肯定希望查看一下新生成的点云了。如果已经安装了pcl,就可以通过:

pcl_viewer data/pointcloud.pcd


来查看新生成的点云。


课后作业


本讲中,我们实现了一个从2D图像到3D点云的转换程序。下一讲,我们将探讨图像的特征点提取与配准。配准过程中,我们需要计算2D图像特征点的空间位置。因此,请你编写一个头文件与一个源文件,实现一个point2dTo3d函数。请在头文件里写这个函数的声明,源文件里给出它的实现,并在cmake中把它编译成一个叫做slam_base的库。(你需要考虑如何定义一个比较好的接口。)这样一来,今后当我们需要计算它时,就只需调用这个函数就可以了。
小萝卜:师兄!这个作业看起来有些难度啊!
师兄:是呀,不能把读者想的太简单嘛。
最后呢,本节用到的源代码仍然可以从我的git里下载到。如果您关于本文有什么问题,也欢迎给我发邮件:gaoxiang12@mails.tsinghua.edu.cn 读者的鼓励就是对我最好的支持!
尊重版权:原文取自半闲居士
分享:
游客
要评论请先登录 或者 注册
返回顶部