(ROS下) realsense 采集 RGBD图像合成点云

 

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

 

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;// 转换点云的数据类型并存储成pcd文件
    pcl::toROSMsg(*cloud,pub_pointcloud);