Wizualizacja pointcloud

Question

Mam punkty 3D z gpu :: reprojectImageTo3D na znalezionym obrazie rozbieżności. Chciałbym teraz wyświetlić ten pointcloud.

Jak przekonwertować znaleziony punktcloud z OpenCV na sensor_msgs/PointCloud2?

Nie muszę publikować pointcloud jest to tylko do wizualizacji debugowania. Czy możliwe jest wyświetlenie go tak jak w przypadku obrazów z węzła? na przykład używając pcl? Byłoby to optymalne, ponieważ moje urządzenie może nie działać dobrze z RViz (w oparciu o odczyty online).

Answer 1

Moim najlepszym przypuszczeniem jest robienie tego w ten sposób i po prostu powtarzam przez cv::mat i wstawiam do pcl, aby przekonwertować na msg, ponieważ nie znalazłem niczego, co robi bezpośrednio.

#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; 
} 

Ten fragment kodu został znaleziony pod numerem apprize.