Блог

У меня есть PointCloud2, поступающий с RGBD-камеры, и я хочу преобразовать его в PCL, сохранив цвета. Я думал об использовании pcl :: PointXYZRGB, но у меня возникли проблемы. Мой код, который спас меня от черно-белого облака точек, был следующим. Вы не могли бы мне помочь?

     pcl::PCLPointCloud2* cloud_tmp = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudPtr_tmp(cloud_tmp);

    pcl_conversions::toPCL(cloud_, *cloud_tmp);
    pcl::PCLPointCloud2* cloud_filtered = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2Ptr cloudFilteredPtr (cloud_filtered);
      
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloudPtr_tmp);
    sor.setLeafSize (0.01, 0.01, 0.01);
    sor.filter (*cloudFilteredPtr);


    pcl::PointCloud<PointType> *xyz_cloud_tmp = new pcl::PointCloud<PointType>;
    pcl::PointCloud<PointType>::Ptr xyzCloudPtr_tmp (xyz_cloud_tmp);

    pcl::fromPCLPointCloud2(*cloudFilteredPtr, *xyzCloudPtr_tmp);

    pcl::PointCloud<PointType> *xyz_cloud_base_link = new pcl::PointCloud<PointType>;
    pcl::PointCloud<PointType>::Ptr xyzCloudPtr_base_link (xyz_cloud_base_link);
    pcl::transformPointCloud (*xyzCloudPtr_tmp, *xyzCloudPtr_base_link, T_base2cam_);

    std::string pcd_file = pcd_start_folder_   "pcd.pcd";
    pcl::io::savePCDFileASCII (pcd_file, *xyzCloudPtr_base_link);

    *boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new 
    pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ> (xyzCloudPtr_base_link);

        
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,1, 1, 1);

    viewer->addCoordinateSystem (1.0);
    while (!viewer->wasStopped ())
    {
    viewer->spinOnce (100);
    }