【双目图像转点云】

用c++编写ROS节点代码，订阅双目相机的两个图像话题，处理这些图像以生成点云消息，并将其发布到另一个话题。

1.创建ROS节点

$ cd ~/catkin_ws/src
$ catkin_create_pkg my_package roscpp sensor_msgs cv_bridge pcl_ros

其中，my_package是您的功能包名称，roscpp、sensor_msgs、cv_bridge、pcl_ros是依赖项。

2.编写ROS节点代码

在my_package/src/目录下创建一个名为my_node.cpp的文件，这是ROS节点的主要功能代码。下面是示例代码，该节点将订阅双目相机的两个图像话题，处理这些图像以生成点云消息，并将其发布到另一个话题。

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/stereo_camera_model.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

class MyNode
{
public:
  MyNode() : nh_("~")
  {
    // Subscribe to the left and right camera topics
    sub_left_image_ = nh_.subscribe("/my_package/left_image", 1, &MyNode::processImages, this);
    sub_right_image_ = nh_.subscribe("/my_package/right_image", 1, &MyNode::processImages, this);

    // Advertise the point cloud topic
    pub_point_cloud_ = nh_.advertise<sensor_msgs::PointCloud2>("/my_package/point_cloud", 1);
  }

  void processImages(const sensor_msgs::ImageConstPtr& left_msg, const sensor_msgs::ImageConstPtr& right_msg)
  {
    // Convert the image messages to OpenCV format
    cv_bridge::CvImagePtr left_cv_ptr, right_cv_ptr;
    try
    {
      left_cv_ptr = cv_bridge::toCvCopy(left_msg, sensor_msgs::image_encodings::BGR8);
      right_cv_ptr = cv_bridge::toCvCopy(right_msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    // Compute the stereo disparity map and point cloud
    image_geometry::StereoCameraModel stereo_camera_model;
    stereo_camera_model.fromCameraInfo(left_info_msg_, right_info_msg_);
    cv::Mat left_image = left_cv_ptr->image;
    cv::Mat right_image = right_cv_ptr->image;
    cv::Mat stereo_image;
    stereo_image = stereo_camera_model.rectifyImage(left_image, right_image);
    cv::Mat disparity;
    stereo_camera_model.computeDisparity(stereo_image, disparity);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    stereo_camera_model.projectDisparityTo3d(disparity, cloud->width, cloud->height, cloud->points);

    // Convert the point cloud to ROS format and publish
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(*cloud, cloud_msg);
    cloud_msg.header.frame_id = "map"; // Set the frame ID
    pub_point_cloud_.publish(cloud_msg);
    }
	private:
	ros::NodeHandle nh_;
	ros::Subscriber sub_left_image_;
	ros::Subscriber sub_right_image_;
	ros::Publisher pub_point_cloud_;
	sensor_msgs::CameraInfo left_info_msg_;
	sensor_msgs::CameraInfo right_info_msg_;
	};
	
	int main(int argc, char** argv)
	{
	ros::init(argc, argv, "my_node");
	MyNode my_node;
	ros::spin();
	return 0;
	}

3. 构建并运行ROS节点

在catkin工作空间中运行catkin_make以构建功能包。然后，使用以下命令启动ROS节点：

$ roscore
$ rosrun my_package my_node