1

我正在尝试将超体素聚类合并到我的 3D 点云应用程序中。但是，点云无法在 RVIZ 上可视化。当我尝试使用 gdb 进行调试时，我意识到即使在该行之后，我的地图对象内部仍然没有数据

std::multimap<uint32_t, uint32_t> supervoxel_adjacency; super.getSupervoxelAdjacency(supervoxel_adjacency);

我参考的教程是这个https://pointclouds.org/documentation/tutorials/supervoxel_clustering.html

问题是什么？它应该用数据填充地图结构供我迭代，但事实并非如此。

void PointCloudProcessor::voxel_clustering(type_pclcloudptr& input, ros::Publisher& Publisher)
  {

      float voxel_resolution = 0.008f;
      float seed_resolution = 0.1f;
      float color_importance = 0.2f;
      float spatial_importance = 0.4f;
      float normal_importance = 1.0f;
    
    pcl::SupervoxelClustering<pcl::PointXYZ> super(voxel_resolution, seed_resolution);
        super.setUseSingleCameraTransform(false);
      super.setInputCloud(input);
      super.setColorImportance(color_importance);
      super.setSpatialImportance(spatial_importance);
      super.setNormalImportance(normal_importance);
    std::map <uint32_t, pcl::Supervoxel<pcl::PointXYZ>::Ptr > supervoxel_clusters;
    if ( !( input->points.empty() ) )
    {
      super.extract(supervoxel_clusters);
      type_pclcloudptr voxel_centroid_cloud = super.getVoxelCentroidCloud();
      pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud();

      std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
        super.getSupervoxelAdjacency(supervoxel_adjacency);
    //  To make a graph of the supervoxel adjacency, we need to iterate through the supervoxel adjacency multimap
        std::multimap<uint32_t, uint32_t>::iterator label_itr = supervoxel_adjacency.begin();
        int idx = 0;
      //PointCloudProcessor::mMarkerArray.markers.resize(supervoxel_adjacency.size());
        for (; label_itr != supervoxel_adjacency.end(); )
        {
            //First get the label
            uint32_t supervoxel_label = label_itr->first;
        Eigen::Vector4f centroid;
        Eigen::Vector4f min;
        Eigen::Vector4f max;
            //Now get the supervoxel corresponding to the label
            pcl::Supervoxel<pcl::PointXYZ>::Ptr supervoxel = supervoxel_clusters.at(supervoxel_label);

            //Now we need to iterate through the adjacent supervoxels and make a point cloud of them
            type_pclcloudptr adjacent_supervoxel_centers;
            std::multimap<uint32_t, uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range(supervoxel_label).first;
            for (; adjacent_itr != supervoxel_adjacency.equal_range(supervoxel_label).second; ++adjacent_itr)
            {
                pcl::Supervoxel<pcl::PointXYZ>::Ptr neighbor_supervoxel = supervoxel_clusters.at(adjacent_itr->second);
          pcl::PointXYZ neighbour_centroid_xyz; 
          pcl::PointXYZRGBA neighbour_centroid_xyzrgba = neighbor_supervoxel->centroid_;
          pcl::copyPoint(neighbour_centroid_xyzrgba, neighbour_centroid_xyz);
                adjacent_supervoxel_centers->push_back(neighbour_centroid_xyz);
            }
        // pcl::compute3DCentroid(*adjacent_supervoxel_centers,centroid);
        // pcl::getMinMax3D(*adjacent_supervoxel_centers, min, max);
        // PointCloudProcessor::mark_cluster(centroid,min, max, idx);
        adjacent_supervoxel_centers->width = adjacent_supervoxel_centers->size ();
        adjacent_supervoxel_centers->height = 1;
        adjacent_supervoxel_centers->is_dense = true;
        adjacent_supervoxel_centers->header.frame_id = "rearAxle";
        Publisher.publish(adjacent_supervoxel_centers);
            //Move iterator forward to next label
            label_itr = supervoxel_adjacency.upper_bound(supervoxel_label);
        idx++;
        } 
     }
}
4

0 回答 0