2、将图像转换为点云

1、它是后面处理地图的基础，最简单的点云地图就是把不同位置的点云进行拼接得到的。

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里看到图像与点云的可视化数据。

3、rgb图像与对应的深度图像，如下

这两张图是来自于数据集nyuv2:http://cs.nyu.edu/~silberman/datasets/ 原图格式是ppm和pgm的,目前格式是png。

在实际的Kinect里（或其他rgb-d相机里）直接采到的RGB图和深度图可能会有些小问题，比如

1）有一些时差（约几到十几个毫秒）。这个时差的存在，会产生“RGB图已经向右转了，深度图还没转”的感觉。

2）光圈中心未对齐。因为深度毕竟是靠另一个相机获取的，所以深度传感器和彩色传感器参数可能不一致。

3）深度图里有很多“洞”。因为RGB-D相机不是万能的，它有一个探测距离的限制啦！太远或太近的东西都是看不见的呢。关于这些“洞”，我们暂时睁一只眼闭一只眼，不去理它。以后我们也可以靠双边bayes滤波器去填这些洞。但是！这是RGB-D相机本身的局限性。软件算法顶多给它修修补补，并不能完全弥补它的缺陷。

在以上给出的图当中，都进行了预处理，可以认为“深度图就是彩色图里每个像素距传感器的距离“。

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

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

上面两张图像给出了机器人外部世界的一个局部信息，假设这个世界由一个点云来描述：X={x1,x2,...xn}。其中每一个点由六个分量组成：r,g,b,x,y,z，分别表示该点的颜色与空间位置；颜色方面，主要由彩色图像记录；而空间位置，可由图像和相机模型、姿态一起计算出来。

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

 

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

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

上面的公式是从（x,y,z）推导到(u,v,d)，反之，也可以已知(u,v,d)，推导到（x,y,z）的方式，推导如下

那么就可以根据上式构建点云了。

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

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

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

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

完成从图像到点云的转换，在上一节讲到的代码根目录/src/ 文件夹中新建一个generatePointCloud.cpp文件：

touch src/generatePointCloud.cpp

在一个工程里可以有多个main函数，因为cmake允许你自己定义编译的过程，我们会把这个cpp也编译成一个可执行的二进制，只要在cmakelists.txt里作相应的更改便行了，更改如下

然后在新建的文件generatePointCloud.cpp里面编写如下代码

//c++标准库 

#include<iostream>

#include<string>

using namespace std;

//opencv库

#include<opencv2/core/core.cpp>

#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=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()来读取图像

//rgb图像是8UC3的彩色图像

rgb=cv::imread(./data/rgb.png);

//depth是16UC1的单通道图像，注意flags设置为-1，表示读取原始数据不做修改

depth=cv::imread("./data/depth.png",-1);

//点云变量

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

PointCloud::Ptr cloud(new PointCloud);

//遍历深度图

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

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

  {

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

ushort d=depth.ptr<ushort>(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<uchar>(m)[n*3];

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

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

//把p加入到点云中

cloud->points.push_back(p);

 }

//设置并保存点云

cloud->height=1;

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 0;

}

分析：

1.我们使用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，就是它离你的真实距离了。

2.在遍历深度图的时候，是按照“先列后行”的顺序，由于m指图像的行，n指图像的列，它和空间点的坐标关系是如下：

对于深度图第m行，第n列的数据可以用depth.ptr<ushort>(m)[n]来获取。其中，cv::Mat的ptr函数会返回指向该图像第m行数据的头指针。然后加上位移n后，这个指针指向的数据就是我们需要读取的数据。

计算三维点坐标的公式我们已经给出过了，代码里原封不动地实现了一遍。我们根据这个公式，新增了一个空间点，并放入了点云中。最后，把整个点云存储为 ./data/pointcloud.pcd 文件。

7、编译并运行

cd build

cmake ..

make

编译通过后，就可以到bin目录下运行generate_pointcloud；运行之后，就可以在data目录下生成点云文件。安装了pcl的话，就可以通过如下命令来查看点云文件

pcl_viewer pointcloud.pcd

TIPS:

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