• pcl--第十一节 点云外接立方体和点云模板匹配

    点云外接立方体（3D物体包容盒）

    使用pcl::MomentOfInertiaEstimation类来获取基于偏心率和惯性矩的描述符。该类还允许提取云的轴对齐和定向的边界框。但是提取的OBB并非最小可能的边界框。

    原理简述

    包围体（包容盒）是一个简单的几何空间，里面包含着复杂形状的物体。为物体添加包围体的目的是快速的进行碰撞检测或者进行精确的碰撞检测之前进行过滤（即当包围体碰撞，才进行精确碰撞检测和处理）。包围体类型包括球体、轴对齐包围盒（AABB）、有向包围盒（OBB）、8-DOP以及凸壳（CONVEX HULL）。

    常见包容盒（ Bounding Volumes）分类：

    • 包容球：SPHERE 用球体包围整个几何体，用于相交测试很方便，但是其紧密型差，周围空隙较大，当物体变形后，包围球需要重新计算。当对象进行旋转运动时，包围球不需要做任何更新，这是包围球的优势，即当几何对象频繁进行旋转运动时，使用包围球效率较高。
    • AABB包容盒：Axially Aligned Bounding Box，3D环境下的AABB盒即一个六面体，每个边都平行于一个坐标平面，较简单，但紧密性较差，当物体旋转、形变之后需要对AABB进行更新。本身的长宽高根据物体大小而定。
    • OBB包容盒：Oriented Bounding Box，此方法紧密型较好，可以降低参与相交测试的包容盒数目，因此性能要优于AABB和包容球。当物体发生旋转，仅需对OBB进行相同的旋转即可，但是当物体形变后，更新OBB的代价较大，故不适用那些软体的对象。

    如上图所示，还有K-DOP，CONVEX HULL等包容盒，越靠右，包容效果好、越紧密。但是检测速度更慢，也更消耗内存资源。

    1. #include 
    2. #include 
    3.  
    4. #include 
    5. #include 
    6. #include 
    7. #include 
    8.  
    9. using namespace std::chrono_literals;
    10.  
    11. int main (int argc, char** argv)
    12. {
    13.   //if (argc != 2)
    14.   //  return (0);
    15.  
    16.   pcl::PointCloud::Ptr cloud (new pcl::PointCloud ());
    17.   if (pcl::io::loadPCDFile ("table_scene_lms400_downsampled.pcd", *cloud) == -1)
    18.     return (-1);
    19.  
    20.   pcl::MomentOfInertiaEstimation  feature_extractor;
    21.   feature_extractor.setInputCloud (cloud);
    22.   feature_extractor.compute ();
    23.  
    24.   std::vector <float> moment_of_inertia;
    25.   std::vector <float> eccentricity;
    26.   pcl::PointXYZ min_point_AABB;
    27.   pcl::PointXYZ max_point_AABB;
    28.   pcl::PointXYZ min_point_OBB;
    29.   pcl::PointXYZ max_point_OBB;
    30.   pcl::PointXYZ position_OBB;
    31.   Eigen::Matrix3f rotational_matrix_OBB;
    32.   float major_value, middle_value, minor_value;
    33.   Eigen::Vector3f major_vector, middle_vector, minor_vector;
    34.   Eigen::Vector3f mass_center;
    35.  
    36.   feature_extractor.getMomentOfInertia (moment_of_inertia);
    37.   feature_extractor.getEccentricity (eccentricity);
    38.   feature_extractor.getAABB (min_point_AABB, max_point_AABB);
    39.   feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    40.   feature_extractor.getEigenValues (major_value, middle_value, minor_value);
    41.   feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
    42.   feature_extractor.getMassCenter (mass_center);
    43.  
    44.   pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    45.   viewer->setBackgroundColor (0, 0, 0);
    46.   viewer->addCoordinateSystem (1);
    47.   viewer->initCameraParameters ();
    48.   viewer->addPointCloud (cloud, "sample cloud");
    49.   viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
    50.   viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "AABB");
    51.  
    52.   Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
    53.   Eigen::Quaternionf quat (rotational_matrix_OBB);
    54.   viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
    55.   viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
    56.  
    57.   pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
    58.   pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
    59.   pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
    60.   pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
    61.   viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
    62.   viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
    63.   viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
    64.  
    65.   while(!viewer->wasStopped())
    66.   {
    67.     viewer->spinOnce (100);
    68.     std::this_thread::sleep_for(100ms);
    69.   }
    70.  
    71.   return (0);
    72. }

    黄色立方体为AABB包容盒，白色立方体为OBB包容盒 

     点云模板匹配

    1. #include 
    2. #include 
    3. #include 
    4. #include 
    5. #include 
    6. #include 
    7. #include 
    8. #include 
    9. #include 
    10. #include 
    11. #include 
    12. #include 
    13. #include 
    14. #include 
    15. #include 
    16.  
    17. class FeatureCloud
    18. {
    19.   public:
    20.     // A bit of shorthand
    21.     typedef pcl::PointCloud PointCloud;
    22.     typedef pcl::PointCloud SurfaceNormals;
    23.     typedef pcl::PointCloud LocalFeatures;
    24.     typedef pcl::search::KdTree SearchMethod;
    25.  
    26.     FeatureCloud () :
    27.       search_method_xyz_ (new SearchMethod),
    28.       normal_radius_ (0.02f),
    29.       feature_radius_ (0.02f)
    30.     {}
    31.  
    32.     ~FeatureCloud () {}
    33.  
    34.     // Process the given cloud
    35.     void
    36.     setInputCloud (PointCloud::Ptr xyz)
    37.     {
    38.       xyz_ = xyz;
    39.       processInput ();
    40.     }
    41.  
    42.     // Load and process the cloud in the given PCD file
    43.     void
    44.     loadInputCloud (const std::string &pcd_file)
    45.     {
    46.       xyz_ = PointCloud::Ptr (new PointCloud);
    47.       pcl::io::loadPCDFile (pcd_file, *xyz_);
    48.       processInput ();
    49.     }
    50.  
    51.     // Get a pointer to the cloud 3D points
    52.     PointCloud::Ptr
    53.     getPointCloud () const
    54.     {
    55.       return (xyz_);
    56.     }
    57.  
    58.     // Get a pointer to the cloud of 3D surface normals
    59.     SurfaceNormals::Ptr
    60.     getSurfaceNormals () const
    61.     {
    62.       return (normals_);
    63.     }
    64.  
    65.     // Get a pointer to the cloud of feature descriptors
    66.     LocalFeatures::Ptr
    67.     getLocalFeatures () const
    68.     {
    69.       return (features_);
    70.     }
    71.  
    72.   protected:
    73.     // Compute the surface normals and local features
    74.     void
    75.     processInput ()
    76.     {
    77.       computeSurfaceNormals ();
    78.       computeLocalFeatures ();
    79.     }
    80.  
    81.     // Compute the surface normals
    82.     void
    83.     computeSurfaceNormals ()
    84.     {
    85.       normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
    86.  
    87.       pcl::NormalEstimation norm_est;
    88.       norm_est.setInputCloud (xyz_);
    89.       norm_est.setSearchMethod (search_method_xyz_);
    90.       norm_est.setRadiusSearch (normal_radius_);
    91.       norm_est.compute (*normals_);
    92.     }
    93.  
    94.     // Compute the local feature descriptors
    95.     void
    96.     computeLocalFeatures ()
    97.     {
    98.       features_ = LocalFeatures::Ptr (new LocalFeatures);
    99.  
    100.       pcl::FPFHEstimation fpfh_est;
    101.       fpfh_est.setInputCloud (xyz_);
    102.       fpfh_est.setInputNormals (normals_);
    103.       fpfh_est.setSearchMethod (search_method_xyz_);
    104.       fpfh_est.setRadiusSearch (feature_radius_);
    105.       fpfh_est.compute (*features_);
    106.     }
    107.  
    108.   private:
    109.     // Point cloud data
    110.     PointCloud::Ptr xyz_;
    111.     SurfaceNormals::Ptr normals_;
    112.     LocalFeatures::Ptr features_;
    113.     SearchMethod::Ptr search_method_xyz_;
    114.  
    115.     // Parameters
    116.     float normal_radius_;
    117.     float feature_radius_;
    118. };
    119.  
    120. class TemplateAlignment
    121. {
    122.   public:
    123.  
    124.     // A struct for storing alignment results
    125.     struct Result
    126.     {
    127.       float fitness_score;
    128.       Eigen::Matrix4f final_transformation;
    129.       PCL_MAKE_ALIGNED_OPERATOR_NEW
    130.     };
    131.  
    132.     TemplateAlignment () :
    133.       min_sample_distance_ (0.05f),
    134.       max_correspondence_distance_ (0.01f*0.01f),
    135.       nr_iterations_ (500)
    136.     {
    137.       // Initialize the parameters in the Sample Consensus Initial Alignment (SAC-IA) algorithm
    138.       sac_ia_.setMinSampleDistance (min_sample_distance_);
    139.       sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
    140.       sac_ia_.setMaximumIterations (nr_iterations_);
    141.     }
    142.  
    143.     ~TemplateAlignment () {}
    144.  
    145.     // Set the given cloud as the target to which the templates will be aligned
    146.     void
    147.     setTargetCloud (FeatureCloud &target_cloud)
    148.     {
    149.       target_ = target_cloud;
    150.       sac_ia_.setInputTarget (target_cloud.getPointCloud ());
    151.       sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
    152.     }
    153.  
    154.     // Add the given cloud to the list of template clouds
    155.     void
    156.     addTemplateCloud (FeatureCloud &template_cloud)
    157.     {
    158.       templates_.push_back (template_cloud);
    159.     }
    160.  
    161.     // Align the given template cloud to the target specified by setTargetCloud ()
    162.     void
    163.     align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
    164.     {
    165.       sac_ia_.setInputSource (template_cloud.getPointCloud ());
    166.       sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());
    167.  
    168.       pcl::PointCloud registration_output;
    169.       sac_ia_.align (registration_output);
    170.  
    171.       result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);
    172.       result.final_transformation = sac_ia_.getFinalTransformation ();
    173.     }
    174.  
    175.     // Align all of template clouds set by addTemplateCloud to the target specified by setTargetCloud ()
    176.     void
    177.     alignAll (std::vector > &results)
    178.     {
    179.       results.resize (templates_.size ());
    180.       for (std::size_t i = 0; i < templates_.size (); ++i)
    181.       {
    182.         align (templates_[i], results[i]);
    183.       }
    184.     }
    185.  
    186.     // Align all of template clouds to the target cloud to find the one with best alignment score
    187.     int
    188.     findBestAlignment (TemplateAlignment::Result &result)
    189.     {
    190.       // Align all of the templates to the target cloud
    191.       std::vector > results;
    192.       alignAll (results);
    193.  
    194.       // Find the template with the best (lowest) fitness score
    195.       float lowest_score = std::numeric_limits<float>::infinity ();
    196.       int best_template = 0;
    197.       for (std::size_t i = 0; i < results.size (); ++i)
    198.       {
    199.         const Result &r = results[i];
    200.         if (r.fitness_score < lowest_score)
    201.         {
    202.           lowest_score = r.fitness_score;
    203.           best_template = (int) i;
    204.         }
    205.       }
    206.  
    207.       // Output the best alignment
    208.       result = results[best_template];
    209.       return (best_template);
    210.     }
    211.  
    212.   private:
    213.     // A list of template clouds and the target to which they will be aligned
    214.     std::vector templates_;
    215.     FeatureCloud target_;
    216.  
    217.     // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
    218.     pcl::SampleConsensusInitialAlignment sac_ia_;
    219.     float min_sample_distance_;
    220.     float max_correspondence_distance_;
    221.     int nr_iterations_;
    222. };
    223.  
    224. // Align a collection of object templates to a sample point cloud
    225. int
    226. main (int argc, char **argv)
    227. {
    228.   if (argc < 3)
    229.   {
    230.     printf ("No target PCD file given!\n");
    231.     return (-1);
    232.   }
    233.  
    234.   // Load the object templates specified in the object_templates.txt file
    235.   std::vector object_templates;
    236.   std::ifstream input_stream (argv[1]);
    237.   object_templates.resize (0);
    238.   std::string pcd_filename;
    239.   while (input_stream.good ())
    240.   {
    241.     std::getline (input_stream, pcd_filename);
    242.     if (pcd_filename.empty () || pcd_filename.at (0) == '#') // Skip blank lines or comments
    243.       continue;
    244.  
    245.     FeatureCloud template_cloud;
    246.     template_cloud.loadInputCloud (pcd_filename);
    247.     object_templates.push_back (template_cloud);
    248.   }
    249.   input_stream.close ();
    250.  
    251.   // Load the target cloud PCD file
    252.   pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
    253.   pcl::io::loadPCDFile (argv[2], *cloud);
    254.  
    255.   // Preprocess the cloud by...
    256.   // ...removing distant points
    257.   const float depth_limit = 1.0;
    258.   pcl::PassThrough pass;
    259.   pass.setInputCloud (cloud);
    260.   pass.setFilterFieldName ("z");
    261.   pass.setFilterLimits (0, depth_limit);
    262.   pass.filter (*cloud);
    263.  
    264.   // ... and downsampling the point cloud
    265.   const float voxel_grid_size = 0.005f;
    266.   pcl::VoxelGrid vox_grid;
    267.   vox_grid.setInputCloud (cloud);
    268.   vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
    269.   //vox_grid.filter (*cloud); // Please see this http://www.pcl-developers.org/Possible-problem-in-new-VoxelGrid-implementation-from-PCL-1-5-0-td5490361.html
    270.   pcl::PointCloud::Ptr tempCloud (new pcl::PointCloud); 
    271.   vox_grid.filter (*tempCloud);
    272.   cloud = tempCloud; 
    273.  
    274.   // Assign to the target FeatureCloud
    275.   FeatureCloud target_cloud;
    276.   target_cloud.setInputCloud (cloud);
    277.  
    278.   // Set the TemplateAlignment inputs
    279.   TemplateAlignment template_align;
    280.   for (std::size_t i = 0; i < object_templates.size (); ++i)
    281.   {
    282.     template_align.addTemplateCloud (object_templates[i]);
    283.   }
    284.   template_align.setTargetCloud (target_cloud);
    285.  
    286.   // Find the best template alignment
    287.   TemplateAlignment::Result best_alignment;
    288.   int best_index = template_align.findBestAlignment (best_alignment);
    289.   const FeatureCloud &best_template = object_templates[best_index];
    290.  
    291.   // Print the alignment fitness score (values less than 0.00002 are good)
    292.   printf ("Best fitness score: %f\n", best_alignment.fitness_score);
    293.  
    294.   // Print the rotation matrix and translation vector
    295.   Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
    296.   Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);
    297.  
    298.   printf ("\n");
    299.   printf ("    | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
    300.   printf ("R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
    301.   printf ("    | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
    302.   printf ("\n");
    303.   printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
    304.  
    305.   // Save the aligned template for visualization
    306.   pcl::PointCloud transformed_cloud;
    307.   pcl::transformPointCloud (*best_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);
    308.   pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud);
    309.  
    310.   return (0);
    311. }

  • 相关阅读:
    【微信小程序】前端+后端 ：第一篇（基于javaweb 案例）
    《ToDesk云电脑vs青椒云性能测试，谁更能实现游戏自由？》
    【node进阶】深入浅出前后端身份验证(下)---JWT
    全球第4大操作系统(鸿蒙)的软件后缀.hap
    VB 语言介绍以及VBA、宏（Macro）的关系
    YOLO目标检测——卫星遥感多类别检测数据集下载分享【含对应voc、coco和yolo三种格式标签】
    【Azure 应用服务】Azure Function 启用 Managed Identity后， Powershell Funciton出现 ERROR: ManagedIdentityCredential authentication failed
    雷达编程实战之功耗优化技术（低功耗）
    企业管理软件使用与择选时要注意五点
    深入解析Java HashMap的Resize源码
  • 原文地址：https://blog.csdn.net/weixin_42398658/article/details/133167211