opencv - 可视化点云

Question

我在找到的视差图像上有来自 gpu::reprojectImageTo3D 的 3D 点。我现在想显示这个点云。

如何将找到的点云从 OpenCV 转换为 sensor_msgs/PointCloud2？

我不需要发布点云，这仅用于调试可视化。是否可以像处理来自节点的图像那样显示它？例如使用 pcl？这将是最佳选择，因为我的设备在 RViz 上的表现可能不佳(基于在线阅读)。

Answer 1

我最好的猜测是这样做，只是遍历 cv::mat 并插入 pcl 以转换为 msg，因为我还没有找到任何直接做的事情。

#include <ros/ros.h>
// point cloud headers
#include <pcl/point_cloud.h>
//Header which contain PCL to ROS and ROS to PCL conversion functions
#include <pcl_conversions/pcl_conversions.h>
//sensor_msgs header for point cloud2
#include <sensor_msgs/PointCloud2.h>
main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_create");
    ROS_INFO("Started PCL publishing node");
    ros::NodeHandle nh;
    //Creating publisher object for point cloud
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);
    //Creating a cloud object
    pcl::PointCloud<pcl::PointXYZ> cloud;
    //Creating a sensor_msg of point cloud
    sensor_msgs::PointCloud2 output;
    //Insert cloud data
    cloud.width  = 50000;
    cloud.height = 2;
    cloud.points.resize(cloud.width * cloud.height);
    //Insert random points on the clouds
    for (size_t i = 0; i < cloud.points.size (); ++i)
    {
        cloud.points[i].x = 512 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].y = 512 * rand () / (RAND_MAX + 1.0f);
        cloud.points[i].z = 512 * rand () / (RAND_MAX + 1.0f);
    }
    //Convert the cloud to ROS message
    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "point_cloud";
    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        //publishing point cloud data
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

此代码片段位于 apprize .