【PCL】（二十六）自定义条件的欧几里得聚类分割点云

（二十六）自定义条件的欧几里得聚类分割点云

以下代码实现自定义条件对点进行欧几里得聚类分割。

conditional_euclidean_clustering.cpp

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/time.h>

#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>

typedef pcl::PointXYZI PointTypeIO;
typedef pcl::PointXYZINormal PointTypeFull;


/*条件函数的格式是固定的：
前两个输入参数的类型必须与pcl::ConditionalEuclideanClustering类中使用的模板化类型相同：包括当前种子点（第一个参数）和当前候选点（第二个参数）的点信息。
第三个输入参数必须是浮点值：为种子点和候选点之间的平方距离。
输出参数必须是布尔值。返回TRUE将把候选点合并到种子点的簇中，否则不会。*/

// 条件函数示例1
bool enforceIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float /*squared_distance*/)
{
      // 根据强度值相近程度进行聚类
      if (std::abs (point_a.intensity - point_b.intensity) < 5.0f)
            return (true);
      else
            return (false);
}
// 条件函数示例2
bool enforceNormalOrIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float /*squared_distance*/)
{
      // 强度值相似或法线方向相似归为一类
      Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap (), point_b_normal = point_b.getNormalVector3fMap ();
      if (std::abs (point_a.intensity - point_b.intensity) < 5.0f)
            return (true);
      if (std::abs (point_a_normal.dot (point_b_normal)) > std::cos (30.0f / 180.0f * static_cast<float> (M_PI)))
            return (true);
      return (false);
}
// 条件函数示例3
bool customRegionGrowing (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
{
      Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.getNormalVector3fMap (), point_b_normal = point_b.getNormalVector3fMap ();
      // 与第二个条件函数相似，但根据点之间的距离具有不同的条件
      if (squared_distance < 10000)
      {
            if (std::abs (point_a.intensity - point_b.intensity) < 8.0f)
                        return (true);
            if (std::abs (point_a_normal.dot (point_b_normal)) > std::cos (30.0f / 180.0f * static_cast<float> (M_PI)))
                        return (true);
      }
      else
      {
            if (std::abs (point_a.intensity - point_b.intensity) < 3.0f)
                        return (true);
      }
      return (false);
}

int main ()
{
      // Data containers used
      pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);
      pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>);
      pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);
      pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>);
      pcl::console::TicToc tt;  // 用于输出计时结果。

      // Load the input point cloud
      std::cerr << "Loading...\n", tt.tic ();
      pcl::io::loadPCDFile ("Statues_5.pcd", *cloud_in);
      std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->size () << " points\n";

      // Downsample the cloud using a Voxel Grid class
      std::cerr << "Downsampling...\n", tt.tic ();
      pcl::VoxelGrid<PointTypeIO> vg;
      vg.setInputCloud (cloud_in);
      vg.setLeafSize (80.0, 80.0, 80.0);
      vg.setDownsampleAllData (true);
      vg.filter (*cloud_out);
      std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->size () << " points\n";

      // Set up a Normal Estimation class and merge data in cloud_with_normals
      std::cerr << "Computing normals...\n", tt.tic ();
      pcl::copyPointCloud (*cloud_out, *cloud_with_normals);
      pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;
      ne.setInputCloud (cloud_out);
      ne.setSearchMethod (search_tree);
      ne.setRadiusSearch (300.0);
      ne.compute (*cloud_with_normals);
      std::cerr << ">> Done: " << tt.toc () << " ms\n";

      // Set up a Conditional Euclidean Clustering class
      std::cerr << "Segmenting to clusters...\n", tt.tic ();  
      pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);  // 类初始化为TRUE。这将允许提取太小或太大的簇
      cec.setInputCloud (cloud_with_normals);
      cec.setConditionFunction (&customRegionGrowing);  //   指定条件函数
      cec.setClusterTolerance (100.0);
      cec.setMinClusterSize (cloud_with_normals->size () / 1000);   // 占云总点不到0.1%的簇被认为太小。
      cec.setMaxClusterSize (cloud_with_normals->size () / 5);   // 占云总点20%以上的簇被认为太大。
      cec.segment (*clusters);
      // 太小的集群或太大的集群不会传递到主输出，而是可以在单独的pcl::Indices据容器中检索，但前提是类已用TRUE初始化。
      cec.getRemovedClusters (small_clusters, large_clusters); 
      std::cerr << ">> Done: " << tt.toc () << " ms\n";



      // Using the intensity channel for lazy visualization of the output
      for (const auto& small_cluster : (*small_clusters))
            for (const auto& j : small_cluster.indices)
                  (*cloud_out)[j].intensity = -2.0;
      for (const auto& large_cluster : (*large_clusters))
            for (const auto& j : large_cluster.indices)
                  (*cloud_out)[j].intensity = +10.0;
      for (const auto& cluster : (*clusters))
      {
            int label = rand () % 8;
            for (const auto& j : cluster.indices)
                  (*cloud_out)[j].intensity = label;
      }

      // Save the output point cloud
      std::cerr << "Saving...\n", tt.tic ();
      pcl::io::savePCDFile ("output.pcd", *cloud_out);
      std::cerr << ">> Done: " << tt.toc () << " ms\n";

      return (0);
}

编译

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(conditional_euclidean_clustering)

find_package(PCL 1.7 REQUIRED)

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

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