一起做RGB-D SLAM (2)

第二讲从图像到点云

　　本讲中，我们将带领读者，编写一个将图像转换为点云的程序。该程序是后期处理地图的基础。最简单的点云地图即是把不同位置的点云进行拼接得到的。

　　当我们使用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，该怎么办啊！

　　师兄：没关系！你可以下载我们给你提供的数据。实际上就是下面两张图片啦！

　　小萝卜：怎么深度图是一团黑的呀！

　　师兄：请睁大眼睛仔细看！怎么可能是黑的！

　　小萝卜：呃……可是确实是黑的啊！

　　师兄：对！这是由于画面里的物体离我们比较近，所以看上去比较黑。但是你实际去读的话可是有数据的哦！

　　重要的备注：

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

　　不过请你放心，在我们给出的这两个图中，都进行了预处理。你可以认为“深度图就是彩色图里每个像素距传感器的距离”啦！

　　师兄：现在，我们要把这两个图转成点云啦，因为计算每个像素的空间点位置，可是后面配准、拼图等一系列事情的基础呢。比如，在配准时，必须知道特征点的3D位置呢，这时候就要用到我们这里讲到的知识啦！

　　小萝卜：听起来很重要的样子！

　　师兄：对！所以请读者朋友务必掌握好这部分的内容啦！

从2D到3D（数学部分）

　　上面两个图像给出了机器人外部世界的一个局部的信息。假设这个世界由一个点云来描述：$X=\{ x_1, \ldots, x_n \}$. 其中每一个点呢，有 $r,g,b,x,y,z$一共6个分量，表示它们的颜色与空间位置。颜色方面，主要由彩色图像记录；而空间位置，可以由图像和相机模型、姿态一起计算出来。

　　对于常规相机，SLAM里使用针孔相机模型（图来自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf ）：

　　简而言之，一个空间点$[ x,y,z ]$和它在图像中的像素坐标$[ u, v, d ]$ ($d$指深度数据) 的对应关系是这样的：

$$ u = \frac{ x \cdot f_x }{z} + c_x $$

$$ v = \frac{ y \cdot f_y }{z} + c_y $$

$$ d = z \cdot s $$

　　其中，$f_x, f_y$指相机在$x,y$两个轴上的焦距，$c_x, c_y$指相机的光圈中心，$s$指深度图的缩放因子。

　　小萝卜：好晕啊！突然冒出这么多个变量！

　　师兄：别急啊，这已经是很简单的模型了，等你熟悉了就不觉得复杂了。

　　这个公式是从$(x,y,z)$推到$(u,v,d)$的。反之，我们也可以把它写成已知$(u,v,d)$，推导$(x,y,z)$的方式。请读者自己推导一下。

　　不，还是我们来推导吧……公式是这样的：

$$ z = d/s $$

$$ x = (u-c_x) \cdot z/f_x $$

$$ y = (v-c_y) \cdot z/ f_y $$

　　怎么样，是不是很简单呢？事实上根据这个公式就可以构建点云啦。

　　通常，我们会把$f_x, f_y, c_x, c_y$这四个参数定义为相机的内参矩阵$C$，也就是相机做好之后就不会变的参数。相机的内参可以用很多方法来标定，详细的步骤比较繁琐，我们这里就不提了。给定内参之后呢，每个点的空间位置与像素坐标就可以用简单的矩阵模型来描述了：

$$ s \cdot \left[ \begin{array}{l} u \\ v \\ 1 \end{array} \right] = C \cdot \left( R \cdot \left[ \begin{array}{l} x \\ y \\ z \end{array} \right] +t \right)$$

　　其中，$R$和$t$是相机的姿态。$R$代表旋转矩阵，$t$代表位移矢量。因为我们现在做的是单幅点云，认为相机没有旋转和平移。所以，把$R$设成单位矩阵$I$，把$t$设成了零。$s$是scaling factor，即深度图里给的数据与实际距离的比例。由于深度图给的都是short (mm单位)，$s$通常为1000。

　　小萝卜：于是就有了上面那个$(u,v,d)$转$(x,y,z)$的公式？

　　师兄：对！真聪明！如果相机发生了位移和旋转，那么只要对这些点进行位移和旋转操作即可。

从2D到3D （编程部分）

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

 touch src/generatePointCloud.cpp

　　小萝卜：师兄！一个工程里可以有好几个main函数么？

　　师兄：对呀，cmake允许你自己定义编译的过程。我们会把这个cpp也编译成一个可执行的二进制，只要在cmakelists.txt里作相应的更改便行了。

　　接下来，请在刚建的文件里输入下面的代码。为保证行文的连贯性，我们先给出完整的代码，然后在重要的地方加以解释。建议新手逐字自己敲一遍，你会掌握得更牢固。

 // C++ 标准库

 #include <iostream>

 #include <string>

 using namespace std;

 // OpenCV 库

 #include <opencv2/core/core.hpp>

 #include <opencv2/highgui/highgui.hpp>

 // PCL 库

 #include <pcl/io/pcd_io.h>

 #include <pcl/point_types.h>

 // 定义点云类型

 typedef pcl::PointXYZRGBA PointT;

 typedef pcl::PointCloud<PointT> PointCloud; 

 // 相机内参

 const double camera_factor = ;

 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", - );

     // 点云变量

     // 使用智能指针，创建一个空点云。这种指针用完会自动释放。

     PointCloud::Ptr cloud ( new PointCloud );

     // 遍历深度图

     for (int m = ; m < depth.rows; m++)

         for (int n=; n < depth.cols; n++)

         {

             // 获取深度图中(m,n)处的值

             ushort d = depth.ptr<ushort>(m)[n];

             // d 可能没有值，若如此，跳过此点

             if (d == )

                 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<uchar>(m)[n*];

             p.g = rgb.ptr<uchar>(m)[n*+];

             p.r = rgb.ptr<uchar>(m)[n*+];

             // 把p加入到点云中

             cloud->points.push_back( p );

         }

     // 设置并保存点云

     cloud->height = ;

     cloud->width = cloud->points.size();

     cout<<"point cloud size = "<<cloud->points.size()<<endl;

     cloud->is_dense = false;

     pcl::io::savePCDFile( "./pointcloud.pcd", *cloud );

     // 清除数据并退出

     cloud->points.clear();

     cout<<"Point cloud saved."<<endl;

     return ;

 }

　　程序运行需要数据。请把上面的那两个图存放在代码根目录/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 = ; m < depth.rows; m++)

      for (int n=; n < 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 pointcloud.pcd

　　来查看新生成的点云。

课后作业

　　本讲中，我们实现了一个从2D图像到3D点云的转换程序。下一讲，我们将探讨图像的特征点提取与配准。配准过程中，我们需要计算2D图像特征点的空间位置。因此，请你编写一个头文件与一个源文件，实现一个point2dTo3d函数。请在头文件里写这个函数的声明，源文件里给出它的实现，并在cmake中把它编译成一个叫做slam_base的库。（你需要考虑如何定义一个比较好的接口。）这样一来，今后当我们需要计算它时，就只需调用这个函数就可以了。

　　小萝卜：师兄！这个作业看起来有些难度啊！

　　师兄：是呀，不能把读者想的太简单嘛。

　　最后呢，本节用到的源代码仍然可以从我的git里下载到。读者的鼓励就是对我最好的支持！

TIPS:

如果你打开点云，只看到红绿蓝三个方块，请按R重置视角。刚才你是站在原点盯着坐标轴看呢。
如果点云没有颜色，请按5显示颜色。
cmake过程可能有PCL的警告，如果你编译成功了，无视它即可。这是程序员的本能。