ROS下利用realsense采集RGBD图像合成点云

摘要：在ROS kinetic下，利用realsense D435深度相机采集校准的RGBD图片，合成点云，在rviz中查看点云，最后保存成pcd文件。

 

一、 各种bug

代码编译成功后，打开rviz添加pointcloud2选项卡，当我订阅合成点云时，可视化失败，选项卡报错：

1）Data size (9394656 bytes) does not match width (640) times height (480) times point_step (32).  Dropping message.

解读：通过  rostopic echo /pointcloud_topic  读取相机节点发布的原始点云的相关数据，可以发现每一帧原始点云的点数量为307200。合成点云的点数量为  9394656 / 32 ,约26万。可以猜测由于某种原因，系统把每一帧合成点云的数据都丢弃了。

原因：我事先给定合成点云的大小为，height = 480，width = 640. 然而在合成点云的过程中，剔除了部分违法值(d=0)，由此导致合成点云的点数量与指定的点数量不匹配，合成点云的数据因此被丢弃。

解决方法：采用如下方法给定点云大小， cloud->height = 1; cloud->width = cloud->points.size(); 

 

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属性是相对于某个坐标系来描述的，因此，需要指定点云的参考坐标系。

解决方法：添加点云的header信息， 

pub_pointcloud.header.frame_id = "camera_color_optical_frame"; 
pub_pointcloud.header.stamp = ros::Time::now();

 

3）将合成的点云存储成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中，即可跳过报错。

 

4）相机内参

由于在彩色图和深度图配准的过程中，选用的是彩色图的坐标系，因此在合成点云（像素坐标在变换到相机坐标）时应该选用彩色图的相机内参。

realsense官方提供了一个应用程序可以查看所有视频流的内参。

gordon@gordon-5577:/usr/local/bin$ ./rs-sensor-control 

如下所示

84 : Color #0 (Video Stream: Y16 640x480@ 60Hz)
85 : Color #0 (Video Stream: BGRA8 640x480@ 60Hz)
86 : Color #0 (Video Stream: RGBA8 640x480@ 60Hz)
87 : Color #0 (Video Stream: BGR8 640x480@ 60Hz)
88 : Color #0 (Video Stream: RGB8 640x480@ 60Hz)
89 : Color #0 (Video Stream: YUYV 640x480@ 60Hz)
90 : Color #0 (Video Stream: Y16 640x480@ 30Hz)
91 : Color #0 (Video Stream: BGRA8 640x480@ 30Hz)
92 : Color #0 (Video Stream: RGBA8 640x480@ 30Hz)
93 : Color #0 (Video Stream: BGR8 640x480@ 30Hz)
94 : Color #0 (Video Stream: RGB8 640x480@ 30Hz)
95 : Color #0 (Video Stream: YUYV 640x480@ 30Hz)
96 : Color #0 (Video Stream: Y16 640x480@ 15Hz)
97 : Color #0 (Video Stream: BGRA8 640x480@ 15Hz)
98 : Color #0 (Video Stream: RGBA8 640x480@ 15Hz)
99 : Color #0 (Video Stream: BGR8 640x480@ 15Hz)
100: Color #0 (Video Stream: RGB8 640x480@ 15