这个算法能够将深度相机采集的2D图像连同对应的深度数据转换成3D点云。先看一下输入图片深度图数据比较小基本上都是黑的没有必要查看运行程序后就可以查看生成的3D点云了up截了两张不同视角的点云因为SLAM相关的程序编译非常困难所以up制作了对应的助手程序可以无痛体验程序效果欢迎大家下载体验https://wwbre.lanzouu.com/b01qfa49yb 密码:6666点击 启动app 就可以运行 将图像转换成点云 的程序。点击 打开输出文件夹 就可以打开对应的文件夹。输入深度相机采集的一张图片rgb.png和对应的深度图depth.png输出pointcloud.pcd PCL点云运行使用slam助手-01程序 一键启动视频演示https://www.bilibili.com/video/BV1AaEr6ME2k/代码// 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::PointCloudPointT 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?highlightimread#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 n0; n depth.cols; n) { // 获取深度图中(m,n)处的值 ushort d depth.ptrushort(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.ptruchar(m)[n*3]; p.g rgb.ptruchar(m)[n*31]; p.r rgb.ptruchar(m)[n*32]; // 把p加入到点云中 cloud-points.push_back( p ); } // 设置并保存点云 cloud-height 1; cloud-width cloud-points.size(); coutpoint cloud size cloud-points.size()endl; cloud-is_dense false; pcl::io::savePCDFile( ./pointcloud.pcd, *cloud ); // 清除数据并退出 cloud-points.clear(); coutPoint cloud saved.endl; return 0; }参考一起做RGB-D SLAM (2) - 半闲居士 - 博客园