完整解决方案：ROS下利用realsense采集RGBD图像合成点云

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

　　摘要：在ROS动力学下，使用realsense D435深度相机采集校准的RGBD图像合成点云，在rviz中查看点云，最后将其保存为pcd文件。

　　一、各种错误

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

　　1）数据大小（9394656字节）与宽度（640)乘以高度（480)乘以point_step（32)。丢弃消息。）。

　　解释：通过rostopic echo / pointcloud_topic读取摄像机节点发布的原创点云的相关数据，可以发现每帧原创点云中的点数为307,200。合成的点云中的点数为9394656/32，约为260,000。可以推测，由于某种原因，系统丢弃了合成点云每一帧的数据。

　　原因：我预先将复合点云的大小设置为高度= 480，宽度= 640.。但是，在合成点云的过程中，删除了一些非法值（d =0)）。合成点云中的点数与指定点数不匹配，因此丢弃了合成点云中的数据。

　　解决方案：使用以下方法指定点云大小，cloud-> height = 1； cloud-> width = cloud-> points.size（）;

　　2）转换xxxxx;

　　解释：通过rostopic echo / pointcloud_topic，发现原创点云数据具有以下信息，

　　header:

seq: 50114

stamp:

secs: 1528439158

nsecs: 557543003

frame_id: "camera_color_optical_frame"

　　由此推断，复合点云缺少参考坐标系header.frame_id。点云的点的XYZ属性是相对于某个坐标系描述的，因此，需要指定点云的参考坐标系。

　　解决方案：添加点云的标题信息，

　　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)

　　在进行了多种搜索之后，我探索了一个窍门并与所有人分享。该错误的真正原因尚未确定，我期待着*敏*感*词*的指导。

　　高博的源代码如下所示，里面的云是pcl的数据类型，

　　pcl :: io :: savePCDFile（“ ./ pointcloud.pcd”，* cloud）;。

　　我的源代码如下所示。首先，通过pcl :: toROSMsg（）将pcl的数据类型转换为ros的数据类型，然后将其写入pcd以跳过错误报告。

　　4）照相机内部参考

　　由于在颜*敏*感*词*和深度图的配准过程中使用了颜*敏*感*词*坐标系，因此在合成点云（像素坐标转换为相机坐标）时应选择颜*敏*感*词*的相机内部参数。

　　Realsense正式提供了一个用于查看所有视频流内部参数的应用程序。

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

　　如下所示

　　84：颜色＃0（视频流：Y16 640x480 @ 60Hz）

　　85：颜色0（视频流：BGRA8 640x480 @ 60Hz）

　　86：颜色＃0（视频流：RGBA8 640x480 @ 60Hz）

　　87：颜色＃0（视频流：BGR8 640x480 @ 60Hz）

　　88：颜色＃0（视频流：RGB8 640x480 @ 60Hz）

　　89：颜色＃0（视频流：YUYV 640x480 @ 60Hz）

　　90：颜色＃0（视频流：Y16 640x480 @ 30Hz）

　　91：颜色＃0（视频流：BGRA8 640x480 @ 30Hz）

　　92：颜色＃0（视频流：RGBA8 640x480 @ 30Hz）

　　93：颜色＃0（视频流：BGR8 640x480 @ 30Hz）

　　94：颜色＃0（视频流：RGB8 640x480 @ 30Hz）

　　95：颜色＃0（视频流：YUYV 640x480 @ 30Hz）

　　96：颜色＃0（视频流：Y16 640x480 @ 15Hz）

　　97：颜色＃0（视频流：BGRA8 640x480 @ 15Hz）

　　98：颜色＃0（视频流：RGBA8 640x480 @ 15Hz）

　　99：颜色＃0（视频流：BGR8 640x480 @ 15Hz）

　　100：颜色＃0（视频流：RGB8 640x480 @ 15Hz）

　　101：颜色＃0（视频流：YUYV 640x480 @ 15Hz）

　　102：颜色＃0（视频流：Y16 640x480 @ 6Hz）

　　103：颜色＃0（视频流：BGRA8 640x480 @ 6Hz）

　　104：颜色＃0（视频流：RGBA8 640x480 @ 6Hz）

　　105：颜色＃0（视频流：BGR8 640x480 @ 6Hz）

　　106：颜色＃0（视频流：RGB8 640x480 @ 6Hz）

　　107：颜色＃0（视频流：YUYV 640x480 @ 6Hz）

　　5）深度图从ROS数据类型转换为CV数据类型

　　查看链接

　　二、程序代码

<p>#include

#include

#include

#include

#include

#include

// PCL 库

#include

#include

#include

#include

// 定义点云类型

typedef pcl::PointCloud PointCloud;

using namespace std;

//namespace enc = sensor_msgs::image_encodings;

// 相机内参

const double camera_factor = 1000;

const double camera_cx = 321.798;

const double camera_cy = 239.607;

const double camera_fx = 615.899;

const double camera_fy = 616.468;

// 全局变量：图像矩阵和点云

cv_bridge::CvImagePtr color_ptr, depth_ptr;

cv::Mat color_pic, depth_pic;

void color_Callback(const sensor_msgs::ImageConstPtr& color_msg)

{

//cv_bridge::CvImagePtr color_ptr;

try

{

cv::imshow("color_view", cv_bridge::toCvShare(color_msg, sensor_msgs::image_encodings::BGR8)->image);

color_ptr = cv_bridge::toCvCopy(color_msg, sensor_msgs::image_encodings::BGR8);

cv::waitKey(1050); // 不断刷新图像，频率时间为int delay，单位为ms

}

catch (cv_bridge::Exception& e )

{

ROS_ERROR("Could not convert from '%s' to 'bgr8'.", color_msg->encoding.c_str());

}

color_pic = color_ptr->image;

// output some info about the rgb image in cv format

coutencoding.c_str());

}

depth_pic = depth_ptr->image;

// output some info about the depth image in cv format

coutwidth = cloud->points.size();

ROS_INFO("point cloud size = %d",cloud->width);

cloud->is_dense = false;// 转换点云的数据类型并存储成pcd文件

pcl::toROSMsg(*cloud,pub_pointcloud);

pub_pointcloud.header.frame_id = "camera_color_optical_frame";

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

pcl::io::savePCDFile("./pointcloud.pcd", pub_pointcloud);

cout