近期，我参考高翔博士的“一起学RGBD-SLAM”教程入门视觉slam。
我合成点云的方法与高博不同。高博是通过存放在本地的彩色图和深度图合成点云；我是通过realsense D435在线采集彩色图和深度图，再合成点云，然后通过/topic把点云发布出来。
代码编译成功后，打开rviz添加pointcloud2选项卡，当我订阅合成点云时，可视化失败，选项卡报错：
1）throw up 292485 points；
解读：通过 rostopic echo /pointcloud_topic 读取相机节点发布的原始点云的相关数据，可以发现每一帧原始点云的点数量为307200。可以猜测由于某种原因，系统把每一帧合成点云的数据都丢弃了。
原因：我事先给定合成点云的大小为，height = 480，width = 640. 然而在合成点云的过程中，剔除了部分违法值(深度数值无穷大或无穷小或不存在)，由此导致合成点云的点数量与指定的点数量不匹配，合成点云的数据因此被丢弃。
2）transform xxxxx；
解读：通过 rostopic echo /pointcloud_topic ，发现原始点云数据具有如下信息，
header: seq: 50114 stamp: secs: 1528439158 nsecs: 557543003 frame_id: "camera_color_optical_frame"
由此推断，合成点云缺失参考坐标系header.frame_id。点云中点的XYZ属性是相对于某个坐标系来描述的，因此，需要指定点云的参考坐标系。
经过上述分析后，着手用代码解决问题。以下为main函数内的代码片段，仅供参考。
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
sensor_msgs::PointCloud2 pub_pointcloud;
// 合成点云的代码与高博一致 while (ros::ok()) { // 遍历深度图 for (int m = 0; m < depth_pic.rows; m++){ for (int n = 0; n < depth_pic.cols; n++){ // 获取深度图中(m,n)处的值 float d = depth_pic.ptr<float>(m)[n]; //ushort d = depth_pic.ptr<ushort>(m)[n]; // d 可能没有值，若如此，跳过此点 if (d == 0) continue; // d 存在值，则向点云增加一个点 pcl::PointXYZRGB 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 = color_pic.ptr<uchar>(m)[n*3]; p.g = color_pic.ptr<uchar>(m)[n*3+1]; p.r = color_pic.ptr<uchar>(m)[n*3+2]; // 把p加入到点云中 cloud->points.push_back( p ); } } // 设置点云属性，采用无序的排列方法存储点云 cloud->height = 1; cloud->width = cloud->points.size(); ROS_INFO("point cloud size = %d",cloud->width); cloud->is_dense = false;
// pcl::toROSMsg()将pcl数据类型转换成ros数据类型 pcl::toROSMsg(*cloud,pub_pointcloud);
// 给定参考坐标系，在已有的相机tf中选取一个 pub_pointcloud.header.frame_id = "camera_color_optical_frame"; pub_pointcloud.header.stamp = ros::Time::now();
// 用pcd存储合成点云 pcl::io::savePCDFile("./pointcloud.pcd", pub_pointcloud);
// 发布合成点云 pointcloud_publisher.publish(pub_pointcloud);
}
我参照高博的代码，将合成点云存储成pcd文件时遇到如下报错：
[ INFO] [1528442016.931874649]: point cloud size = 0 terminate called after throwing an instance of 'pcl::IOException' what(): : [pcl::PCDWriter::writeASCII] Input point cloud has no data! Aborted (core dumped)
经过多方查找，摸索了一步trick，分享给大家。真实报错原因仍未查明，期待前辈的指点。
高博的源代码如下所示，里面的cloud是pcl的数据类型，
pcl::io::savePCDFile( "./pointcloud.pcd", *cloud ); 。
我的源代码如以上所示，先通过 pcl::toROSMsg() 将pcl的数据类型转换成ros的数据类型，再写入pcd中，即可跳过报错。