RGB-D SLAM程序框架总结3.点云显示

首先根据某个像素点的像素坐标和深度值计算出它的空间坐标（x,y,z）
论证过程见http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf
可以得到坐标转换公式,由(u,v,d)到(x,y,z)
z=d/s

x=(u−cx)⋅z/fx

y=(v−cy)⋅z/fy

这样就能通过深度信息计算出对应点的空间坐标了
此处显示的过程中发现有颠倒的现象存在，为了直观把x坐标和z坐标取反得到容易观察的视图
也就是把公式变成这样
z=-d/s

x=-(u−cx)⋅z/fx

y=(v−cy)⋅z/fy
（不过不建议在点云拼接的过程中做，这样会改变点云之间的变换关系，不过仍然可以对拼接完成后的点云进行这样的简化处理，相应的程序在下一节中有提到）
点云显示模块使用到了PCL的功能，点云初始化

//定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

//使用智能指针，创建一个空点云。这种指针用网会自动释放
	PointCloud::Ptr cloud(new PointCloud);

点云生成

PointT p;
//从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_r.pcd",*cloud);

参考文献https://www.cnblogs.com/gaoxiang12/p/4633316.html系列教程
毛星云 《OpenCV3编程入门》