第二讲 从图像到点云 笔记
本讲中，我们将带领读者，编写一个将3D图像转换为3D点云的程序。该程序是后期处理地图的基础。最简单的点云地图即是把不同位置的点云进行拼接得到的。
当我们使用RGB-D相机时，会从相机里读到两种数据：彩色图像和深度图像。
由于没有相机，我们采用的深度图和RGB图。我们要把这两个图转成点云啦，因为计算每个像素的空间点位置，可是后面配准、拼图等一系列事情的基础呢。比如，在配准时，必须知道特征点的3D位置呢，这时候就要用到我们这里讲到的知识啦！
-----------------------------------------------------------------------------------------------------------------------------------------------------
从2D到3D（数学部分）
如果相机发生了位移和旋转，那么只要对这些点进行位移和旋转操作即可。
-----------------------------------------------------------------------------------------------------------------------------------------------------
从2D到3D（编程部分）
下面，我们来实现一个程序，完成从图像到点云的转换。请在上一节讲到的 代码根目录/src/ 文件夹中新建一个generatePointCloud.cpp文件：
// 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 = 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 < 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; }
程序运行需要数据。请把上面的那两个图存放在 代码根目录/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，就是它离你的真实距离了。
接下来，我们按照“先列后行”的顺序，遍历了整张深度图。在这个双重循环中：
1 for (int m = 0; m < depth.rows; m++)
2 for (int n=0; 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} )
2.编译并运行
1 cd build
2 cmake .. #..代表代码根目录 /home/louis/slam
3 make
4 cd ..
如果编译通过，就可在bin目录下找到新写的二进制：generate_pointcloud 运行它：
bin/generate_pointcloud
即可在data目录下生成点云文件。现在，你肯定希望查看一下新生成的点云了。如果已经安装了pcl，就可以通过：
1 cd ../data
2 pcl_viewer pointcloud.pcd
太开心了！！终于有结果了！
