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

    header: 
      seq: 
      stamp: 
    secs: 
    nsecs: 
      frame_id: "camera_color_optical_frame"

    "camera_color_optical_frame"; 
    pub_pointcloud.header.stamp

    5577

    #include 
    #include 
    #include 
    #include 
    #include // PCL 库
    #include 
    #include 
    #include // 定义点云类型 PointCloud; 
    
    usingnamespace std;
    //namespace enc = sensor_msgs::image_encodings;
    
    // 相机内参constdouble1000;
    constdouble321.798;
    constdouble239.607;
    constdouble615.899;
    constdouble616.468;
    
    // 全局变量：图像矩阵和点云cv_bridge::CvImagePtr color_ptr, depth_ptr;
    cv::Mat color_pic, depth_pic;
    
    voidconst color_msg)
    {
      //cv_bridge::CvImagePtr color_ptr;try
      {
    cv::imshow("color_view"image);
    color_ptr  cv_bridge::toCvCopy(color_msg, sensor_msgs::image_encodings::BGR8);    
    
    cv::waitKey(1050// 不断刷新图像，频率时间为int delay，单位为ms  }
      catch e )
      {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'."encoding.c_str());
      }
      color_pic image;
    
      // output some info about the rgb image in cv format"output some info about the rgb image in cv format"endl;
      cout"rows of the rgb image = "endl; 
      cout"cols of the rgb image = "endl; 
      cout"type of rgb_pic's element = "endl; 
    }
    
    
    voidconst depth_msg)
    {
      //cv_bridge::CvImagePtr depth_ptr;try
      {
    //cv::imshow("depth_view", cv_bridge::toCvShare(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1)->image);
    //depth_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_16UC1); "depth_view"image);
    depth_ptr  cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_32FC1); 
    
    cv::waitKey(1050);
      }
      catch e)
      {
    ROS_ERROR("Could not convert from '%s' to 'mono16'."encoding.c_str());
      }
    
      depth_pic image;
    
      // output some info about the depth image in cv format"output some info about the depth image in cv format"endl;
      cout"rows of the depth image = "endl; 
      cout"cols of the depth image = "endl; 
      cout"type of depth_pic's element = "endl; 
    }
    intintcharargv)
    {
      ros::init(argc, argv, "image_listener");
      ros::NodeHandle nh;
      cv::namedWindow("color_view");
      cv::namedWindow("depth_view");
      cv::startWindowThread();
      image_transport::ImageTransport it(nh);
      image_transport::Subscriber sub "/camera/color/image_raw"1, color_Callback);
      image_transport::Subscriber sub1 "/camera/aligned_depth_to_color/image_raw"1, depth_Callback);"generated_pc"1);
     // 点云变量
      // 使用智能指针，创建一个空点云。这种指针用完会自动释放。new PointCloud );
      sensor_msgs::PointCloud2 pub_pointcloud;
    
      double1.0// 1HZ，1秒发1次 // use to regulate loop rate 
      cout"depth value of depth map : "endl;
    
      while (ros::ok()) {
    // 遍历深度图forint0){
      forint0){
          // 获取深度图中(m,n)处的值floatfloat(m)[n];//ushort d = depth_pic.ptr<ushort>(m)[n];
          // d 可能没有值，若如此，跳过此点if0)
             continue;
          // d 存在值，则向点云增加一个点          pcl::PointXYZRGB p;
    
          // 计算这个点的空间坐标double camera_factor;
          p.x  camera_fx;
          p.y  camera_fy;
            
          // 从rgb图像中获取它的颜色
          // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色3];
          p.g 31];
          p.r 32];
        
          // 把p加入到点云中points.push_back( p );
      }
    }
        
    
    // 设置并保存点云1;
    cloudpoints.size();
    ROS_INFO("point cloud size = %d"width);false;// 转换点云的数据类型并存储成pcd文件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"publish point_cloud height = "endl;
    cout"publish point_cloud width = "endl;
    
    // 发布合成点云和原始点云    pointcloud_publisher.publish(pub_pointcloud);
    ori_pointcloud_publisher.publish(cloud_msg);
    
    // 清除数据并退出points.clear();
    
    ros::spinOnce(); //allow data update from callback; // wait for remainder of specified period;   }
    
      cv::destroyWindow("color_view");
      cv::destroyWindow("depth_view");
    }