（二十九）超体素聚类分割点云

论文：Voxel Cloud Connectivity Segmentation - Supervoxels for Point Clouds

supervoxel_clustering.cpp

#include <pcl/console/parse.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/supervoxel_clustering.h>

//VTK include needed for drawing graph lines
#include <vtkPolyLine.h>

// Types
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointNCloudT;
typedef pcl::PointXYZL PointLT;
typedef pcl::PointCloud<PointLT> PointLCloudT;

void addSupervoxelConnectionsToViewer (PointT &supervoxel_center,
                                       PointCloudT &adjacent_supervoxel_centers,
                                       std::string supervoxel_name,
                                       pcl::visualization::PCLVisualizer::Ptr & viewer);


int main (int argc, char ** argv)
{
      if (argc < 2)
      {
            pcl::console::print_error ("Syntax is: %s <pcd-file> \n "
                                          "--NT Dsables the single cloud transform \n"
                                          "-v <voxel resolution>\n-s <seed resolution>\n"
                                          "-c <color weight> \n-z <spatial weight> \n"
                                          "-n <normal_weight>\n", argv[0]);
            return (1);
      }


      PointCloudT::Ptr cloud (new PointCloudT);
      pcl::console::print_highlight ("Loading point cloud...\n");
      if (pcl::io::loadPCDFile<PointT> (argv[1], *cloud))
      {
            pcl::console::print_error ("Error loading cloud file!\n");
            return (1);
      }
     /*
      --NT禁用单视图变换（仅影响有组织云）
      -v设置体素大小，决定基础八叉树结构的叶大小（以米为单位）
      -s设置种子大小，决定超体素的大小（以米为单位）
      -c设置颜色影响超体素的形状的权重
      -z设置空间项的权重-值越高，超体素越规则
      -n设置曲面法线影响超体素的形状的权重
      */

      bool disable_transform = pcl::console::find_switch (argc, argv, "--NT");

      float voxel_resolution = 0.008f;
      bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");
      if (voxel_res_specified)
            pcl::console::parse (argc, argv, "-v", voxel_resolution);

      float seed_resolution = 0.1f;
      bool seed_res_specified = pcl::console::find_switch (argc, argv, "-s");
      if (seed_res_specified)
            pcl::console::parse (argc, argv, "-s", seed_resolution);

      float color_importance = 0.2f;
      if (pcl::console::find_switch (argc, argv, "-c"))
            pcl::console::parse (argc, argv, "-c", color_importance);

      float spatial_importance = 0.4f;
      if (pcl::console::find_switch (argc, argv, "-z"))
            pcl::console::parse (argc, argv, "-z", spatial_importance);

      float normal_importance = 1.0f;
      if (pcl::console::find_switch (argc, argv, "-n"))
            pcl::console::parse (argc, argv, "-n", normal_importance);


 


      // 超体素聚类
      pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);
      if (disable_transform) // 如果收入是有组织的云，而该云的相机坐标不在（0,0,0）且深度不在正Z，则必须将use_transform设置为false
            super.setUseSingleCameraTransform (false); 
      super.setInputCloud (cloud);
      super.setColorImportance (color_importance);
      super.setSpatialImportance (spatial_importance);
      super.setNormalImportance (normal_importance);

      std::map <std::uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;

      pcl::console::print_highlight ("Extracting supervoxels!\n");
      super.extract (supervoxel_clusters);
      pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());



      // 超体素可视化
      pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
      viewer->setBackgroundColor (0, 0, 0);
      // voxel_centroid_cloud包含由体素质心组成的云
      PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
      viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
      viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
      viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "voxel centroids");
      //labeled_voxel_cloud 是根据其超体素标签（随机颜色）着色的体素。
      PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();
      viewer->addPointCloud (labeled_voxel_cloud, "labeled voxels");
      viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "labeled voxels");
      // sv_normal_cloud包含一个超体素法线云，
      PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
      //We have this disabled so graph is easy to see, uncomment to see supervoxel normals
      //viewer->addPointCloudNormals<PointNormal> (sv_normal_cloud,1,0.05f, "supervoxel_normals");
      

      pcl::console::print_highlight ("Getting supervoxel adjacency\n");
      std::multimap<std::uint32_t, std::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
      for (auto label_itr = supervoxel_adjacency.cbegin (); label_itr != supervoxel_adjacency.cend (); )
      {
            //First get the label  
            std::uint32_t supervoxel_label = label_itr->first;
            //Now get the supervoxel corresponding to the label
            pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);

            //Now we need to iterate through the adjacent supervoxels and make a point cloud of them
            PointCloudT adjacent_supervoxel_centers;
            for (auto adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first; adjacent_itr!=supervoxel_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr)
            {
                  pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_itr->second);
                  adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_);
            }
            //Now we make a name for this polygon
            std::stringstream ss;
            ss << "supervoxel_" << supervoxel_label;
            //This function is shown below, but is beyond the scope of this tutorial - basically it just generates a "star" polygon mesh from the points given
            addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer);
            //Move iterator forward to next label
            label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);
      }

      while (!viewer->wasStopped ())
      {
            viewer->spinOnce (100);
      }
      return (0);
      }

void  addSupervoxelConnectionsToViewer (PointT &supervoxel_center,PointCloudT &adjacent_supervoxel_centers,
                               std::string supervoxel_name,pcl::visualization::PCLVisualizer::Ptr & viewer)
{
      vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
      vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
      vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();

      //Iterate through all adjacent points, and add a center point to adjacent point pair
      for (auto adjacent_itr = adjacent_supervoxel_centers.begin (); adjacent_itr != adjacent_supervoxel_centers.end (); ++adjacent_itr)
      {
            points->InsertNextPoint (supervoxel_center.data);
            points->InsertNextPoint (adjacent_itr->data);
      }
      // Create a polydata to store everything in
      vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
      // Add the points to the dataset
      polyData->SetPoints (points);
      polyLine->GetPointIds  ()->SetNumberOfIds(points->GetNumberOfPoints ());
      for(unsigned int i = 0; i < points->GetNumberOfPoints (); i++)
      polyLine->GetPointIds ()->SetId (i,i);
      cells->InsertNextCell (polyLine);
      // Add the lines to the dataset
      polyData->SetLines (cells);
      viewer->addModelFromPolyData (polyData,supervoxel_name);
}

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(supervoxel_clustering)

find_package(PCL 1.8 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (supervoxel_clustering supervoxel_clustering.cpp)
target_link_libraries (supervoxel_clustering ${PCL_LIBRARIES})

数据样例

编译并运行：

./supervoxel_clustering milk_cartoon_all_small_clorox.pcd --NT -s 0.47

./supervoxel_clustering milk_cartoon_all_small_clorox.pcd --NT -s 0.1