这次我们要解释如何以pcl_recognition模块来进行3D物体识别。特别地，它解释了怎么使用相关组算法为了聚类那些从3D描述器算法里面把当前的场景与模型进行匹配的相关点对点的匹配。(长难句)。对于每一次聚类，描绘了一个在场景中的可能模型实例，相关组算法也输出标识6DOF位姿估计的转换矩阵。
代码
#include <pcl/io/pcd_io.h> #include <pcl/point_cloud.h> #include <pcl/correspondence.h> #include <pcl/features/normal_3d_omp.h> #include <pcl/features/shot_omp.h> #include <pcl/features/board.h> #include <pcl/filters/uniform_sampling.h> #include <pcl/recognition/cg/hough_3d.h> #include <pcl/recognition/cg/geometric_consistency.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/kdtree/kdtree_flann.h> #include <pcl/kdtree/impl/kdtree_flann.hpp> #include <pcl/common/transforms.h> #include <pcl/console/parse.h> typedef pcl::PointXYZRGBA PointType; typedef pcl::Normal NormalType; typedef pcl::ReferenceFrame RFType; typedef pcl::SHOT352 DescriptorType; std::string model_filename_; std::string scene_filename_; //Algorithm params bool show_keypoints_ (false); bool show_correspondences_ (false); bool use_cloud_resolution_ (false); bool use_hough_ (true); float model_ss_ (0.01f); float scene_ss_ (0.03f); float rf_rad_ (0.015f); float descr_rad_ (0.02f); float cg_size_ (0.01f); float cg_thresh_ (5.0f); void showHelp (char *filename) { std::cout << std::endl; std::cout << "***************************************************************************" << std::endl; std::cout << "* *" << std::endl; std::cout << "* Correspondence Grouping Tutorial - Usage Guide *" << std::endl; std::cout << "* *" << std::endl; std::cout << "***************************************************************************" << std::endl << std::endl; std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl; std::cout << "Options:" << std::endl; std::cout << " -h: Show this help." << std::endl; std::cout << " -k: Show used keypoints." << std::endl; std::cout << " -c: Show used correspondences." << std::endl; std::cout << " -r: Compute the model cloud resolution and multiply" << std::endl; std::cout << " each radius given by that value." << std::endl; std::cout << " --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl; std::cout << " --model_ss val: Model uniform sampling radius (default 0.01)" << std::endl; std::cout << " --scene_ss val: Scene uniform sampling radius (default 0.03)" << std::endl; std::cout << " --rf_rad val: Reference frame radius (default 0.015)" << std::endl; std::cout << " --descr_rad val: Descriptor radius (default 0.02)" << std::endl; std::cout << " --cg_size val: Cluster size (default 0.01)" << std::endl; std::cout << " --cg_thresh val: Clustering threshold (default 5)" << std::endl << std::endl; } void parseCommandLine (int argc, char *argv[]) { //Show help if (pcl::console::find_switch (argc, argv, "-h")) { showHelp (argv[0]); exit (0); } //Model & scene filenames std::vector<int> filenames; filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); if (filenames.size () != 2) { std::cout << "Filenames missing.\n"; showHelp (argv[0]); exit (-1); } model_filename_ = argv[filenames[0]]; scene_filename_ = argv[filenames[1]]; //Program behavior if (pcl::console::find_switch (argc, argv, "-k")) { show_keypoints_ = true; } if (pcl::console::find_switch (argc, argv, "-c")) { show_correspondences_ = true; } if (pcl::console::find_switch (argc, argv, "-r")) { use_cloud_resolution_ = true; } std::string used_algorithm; if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1) { if (used_algorithm.compare ("Hough") == 0) { use_hough_ = true; }else if (used_algorithm.compare ("GC") == 0) { use_hough_ = false; } else { std::cout << "Wrong algorithm name.\n"; showHelp (argv[0]); exit (-1); } } //General parameters pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_); pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_); pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_); pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_); pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_); pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_); } double computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud) { double res = 0.0; int n_points = 0; int nres; std::vector<int> indices (2); std::vector<float> sqr_distances (2); pcl::search::KdTree<PointType> tree; tree.setInputCloud (cloud); for (size_t i = 0; i < cloud->size (); ++i) { if (! pcl_isfinite ((*cloud)[i].x)) { continue; } //Considering the second neighbor since the first is the point itself. nres = tree.nearestKSearch (i, 2, indices, sqr_distances); if (nres == 2) { res += sqrt (sqr_distances[1]); ++n_points; } } if (n_points != 0) { res /= n_points; } return res; } int main (int argc, char *argv[]) { parseCommandLine (argc, argv); pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ()); pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ()); pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ()); pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ()); pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ()); pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ()); pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ()); pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ()); // // Load clouds // if (pcl::io::loadPCDFile (model_filename_, *model) < 0) { std::cout << "Error loading model cloud." << std::endl; showHelp (argv[0]); return (-1); } if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0) { std::cout << "Error loading scene cloud." << std::endl; showHelp (argv[0]); return (-1); } // // Set up resolution invariance // if (use_cloud_resolution_) { float resolution = static_cast<float> (computeCloudResolution (model)); if (resolution != 0.0f) { model_ss_ *= resolution; scene_ss_ *= resolution; rf_rad_ *= resolution; descr_rad_ *= resolution; cg_size_ *= resolution; } std::cout << "Model resolution: " << resolution << std::endl; std::cout << "Model sampling size: " << model_ss_ << std::endl; std::cout << "Scene sampling size: " << scene_ss_ << std::endl; std::cout << "LRF support radius: " << rf_rad_ << std::endl; std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl; std::cout << "Clustering bin size: " << cg_size_ << std::endl << std::endl; } // // Compute Normals // pcl::NormalEstimationOMP<PointType, NormalType> norm_est; norm_est.setKSearch (10); norm_est.setInputCloud (model); norm_est.compute (*model_normals); norm_est.setInputCloud (scene); norm_est.compute (*scene_normals); // // Downsample Clouds to Extract keypoints // pcl::UniformSampling<PointType> uniform_sampling; uniform_sampling.setInputCloud (model); uniform_sampling.setRadiusSearch (model_ss_); uniform_sampling.filter (*model_keypoints); std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl; uniform_sampling.setInputCloud (scene); uniform_sampling.setRadiusSearch (scene_ss_); uniform_sampling.filter (*scene_keypoints); std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl; // // Compute Descriptor for keypoints // pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est; descr_est.setRadiusSearch (descr_rad_); descr_est.setInputCloud (model_keypoints); descr_est.setInputNormals (model_normals); descr_est.setSearchSurface (model); descr_est.compute (*model_descriptors); descr_est.setInputCloud (scene_keypoints); descr_est.setInputNormals (scene_normals); descr_est.setSearchSurface (scene); descr_est.compute (*scene_descriptors); // // Find Model-Scene Correspondences with KdTree // pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ()); pcl::KdTreeFLANN<DescriptorType> match_search; match_search.setInputCloud (model_descriptors); // For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector. for (size_t i = 0; i < scene_descriptors->size (); ++i) { std::vector<int> neigh_indices (1); std::vector<float> neigh_sqr_dists (1); if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs { continue; } int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists); if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design) { pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]); model_scene_corrs->push_back (corr); } } std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl; // // Actual Clustering // std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations; std::vector<pcl::Correspondences> clustered_corrs; // Using Hough3D if (use_hough_) { // // Compute (Keypoints) Reference Frames only for Hough // pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ()); pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ()); pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est; rf_est.setFindHoles (true); rf_est.setRadiusSearch (rf_rad_); rf_est.setInputCloud (model_keypoints); rf_est.setInputNormals (model_normals); rf_est.setSearchSurface (model); rf_est.compute (*model_rf); rf_est.setInputCloud (scene_keypoints); rf_est.setInputNormals (scene_normals); rf_est.setSearchSurface (scene); rf_est.compute (*scene_rf); // Clustering pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer; clusterer.setHoughBinSize (cg_size_); clusterer.setHoughThreshold (cg_thresh_); clusterer.setUseInterpolation (true); clusterer.setUseDistanceWeight (false); clusterer.setInputCloud (model_keypoints); clusterer.setInputRf (model_rf); clusterer.setSceneCloud (scene_keypoints); clusterer.setSceneRf (scene_rf); clusterer.setModelSceneCorrespondences (model_scene_corrs); //clusterer.cluster (clustered_corrs); clusterer.recognize (rototranslations, clustered_corrs); } else // Using GeometricConsistency { pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer; gc_clusterer.setGCSize (cg_size_); gc_clusterer.setGCThreshold (cg_thresh_); gc_clusterer.setInputCloud (model_keypoints); gc_clusterer.setSceneCloud (scene_keypoints); gc_clusterer.setModelSceneCorrespondences (model_scene_corrs); //gc_clusterer.cluster (clustered_corrs); gc_clusterer.recognize (rototranslations, clustered_corrs); } // // Output results // std::cout << "Model instances found: " << rototranslations.size () << std::endl; for (size_t i = 0; i < rototranslations.size (); ++i) { std::cout << "\n Instance " << i + 1 << ":" << std::endl; std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl; // Print the rotation matrix and translation vector Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0); Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3); printf ("\n"); printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2)); printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2)); printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2)); printf ("\n"); printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2)); } // // Visualization // pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping"); viewer.addPointCloud (scene, "scene_cloud"); pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ()); pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ()); if (show_correspondences_ || show_keypoints_) { // We are translating the model so that it doesn't end in the middle of the scene representation pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128); viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model"); } if (show_keypoints_) { pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255); viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints"); pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255); viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints"); } for (size_t i = 0; i < rototranslations.size (); ++i) { pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ()); pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]); std::stringstream ss_cloud; ss_cloud << "instance" << i; pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0); viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ()); if (show_correspondences_) { for (size_t j = 0; j < clustered_corrs[i].size (); ++j) { std::stringstream ss_line; ss_line << "correspondence_line" << i << "_" << j; PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query); PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match); // We are drawing a line for each pair of clustered correspondences found between the model and the scene viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ()); } } } while (!viewer.wasStopped ()) { viewer.spinOnce (); } return (0); }
帮助函数
这个函数将会打印出几个命令行的简短选项的解释。
void showHelp (char *filename) { std::cout << std::endl; std::cout << "***************************************************************************" << std::endl; std::cout << "* *" << std::endl; std::cout << "* Correspondence Grouping Tutorial - Usage Guide *" << std::endl; std::cout << "* *" << std::endl; std::cout << "***************************************************************************" << std::endl << std::endl; std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl; std::cout << "Options:" << std::endl; std::cout << " -h: Show this help." << std::endl; std::cout << " -k: Show used keypoints." << std::endl; std::cout << " -c: Show used correspondences." << std::endl; std::cout << " -r: Compute the model cloud resolution and multiply" << std::endl; std::cout << " each radius given by that value." << std::endl; std::cout << " --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl; std::cout << " --model_ss val: Model uniform sampling radius (default 0.01)" << std::endl; std::cout << " --scene_ss val: Scene uniform sampling radius (default 0.03)" << std::endl; std::cout << " --rf_rad val: Reference frame radius (default 0.015)" << std::endl; std::cout << " --descr_rad val: Descriptor radius (default 0.02)" << std::endl; std::cout << " --cg_size val: Cluster size (default 0.01)" << std::endl; std::cout << " --cg_thresh val: Clustering threshold (default 5)" << std::endl << std::endl; }
第二个函数，解析命令行参数为了设置正确的参数来执行。
void parseCommandLine (int argc, char *argv[]) { //Show help if (pcl::console::find_switch (argc, argv, "-h")) { showHelp (argv[0]); exit (0); } //Model & scene filenames std::vector<int> filenames; filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd"); if (filenames.size () != 2) { std::cout << "Filenames missing.\n"; showHelp (argv[0]); exit (-1); } model_filename_ = argv[filenames[0]]; scene_filename_ = argv[filenames[1]]; //Program behavior if (pcl::console::find_switch (argc, argv, "-k")) { show_keypoints_ = true; } if (pcl::console::find_switch (argc, argv, "-c")) { show_correspondences_ = true; } if (pcl::console::find_switch (argc, argv, "-r")) { use_cloud_resolution_ = true; } std::string used_algorithm; if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1) { if (used_algorithm.compare ("Hough") == 0) { use_hough_ = true; }else if (used_algorithm.compare ("GC") == 0) { use_hough_ = false; } else { std::cout << "Wrong algorithm name.\n"; showHelp (argv[0]); exit (-1); } } //General parameters pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_); pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_); pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_); pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_); pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_); pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_); }
你可以用两个相应的命令行选项来进行聚类算法的切换。--algorithm(Hough|GC)
Hough(默认)
这是一个以3DHough决策为基础的算法
GC
这是一个geometric consistency(GC) 几何一致聚类算法，执行简单的几何约束在一对相关的物体之间。
一些别的比较有趣的选项 -k -c 和 -r
-k 显示了计算相关度的关键点，以蓝色覆盖的形式显示。
-c画了一条线连接两幅图里面相关的地方。
-r估计模型点的空间分辨率，然后考虑把半径作为点云分辨率。因此获取一些分辨率不变量将会是有用的，当我们获取同样的命令行从不同的点云里面。
下面这个函数实现了某个点云的空间分辨率计算，通过对每个点云里面的点和其最近邻平均距离。
double computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud) { double res = 0.0; int n_points = 0; int nres; std::vector<int> indices (2); std::vector<float> sqr_distances (2); pcl::search::KdTree<PointType> tree; tree.setInputCloud (cloud); for (size_t i = 0; i < cloud->size (); ++i) { if (! pcl_isfinite ((*cloud)[i].x)) { continue; } //Considering the second neighbor since the first is the point itself. nres = tree.nearestKSearch (i, 2, indices, sqr_distances); if (nres == 2) { res += sqrt (sqr_distances[1]); ++n_points; } } if (n_points != 0) { res /= n_points; } return res; }
聚类通道
这是我们主要的函数，执行实际的聚类操作。
parseCommandLine (argc, argv); if (pcl::io::loadPCDFile (model_filename_, *model) < 0) { std::cout << "Error loading model cloud." << std::endl; showHelp (argv[0]); return (-1); } if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0) { std::cout << "Error loading scene cloud." << std::endl; showHelp (argv[0]); return (-1); }
第二步，只有分辨率不变这个标志被enable,程序会调整会在下一节里面使用的半径，把它们和模型分辨率相乘。
if (use_cloud_resolution_) { float resolution = static_cast<float> (computeCloudResolution (model)); if (resolution != 0.0f) { model_ss_ *= resolution; scene_ss_ *= resolution; rf_rad_ *= resolution; descr_rad_ *= resolution; cg_size_ *= resolution; } std::cout << "Model resolution: " << resolution << std::endl; std::cout << "Model sampling size: " << model_ss_ << std::endl; std::cout << "Scene sampling size: " << scene_ss_ << std::endl; std::cout << "LRF support radius: " << rf_rad_ << std::endl; std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl; std::cout << "Clustering bin size: " << cg_size_ << std::endl << std::endl; }
接下去，将会计算模型和场景的每个点的法线，通过NormalEstiminationOMP估计器，使用10个最近邻。
pcl::NormalEstimationOMP<PointType, NormalType> norm_est; norm_est.setKSearch (10); norm_est.setInputCloud (model); norm_est.compute (*model_normals); norm_est.setInputCloud (scene); norm_est.compute (*scene_normals);
接下去我们对每个点云降低取样来找到少量的关键点。UniformSampling里面要使用的参数要么是命令行里面进行设置要么就用默认值。
pcl::UniformSampling<PointType> uniform_sampling; uniform_sampling.setInputCloud (model); uniform_sampling.setRadiusSearch (model_ss_); uniform_sampling.filter (*model_keypoints); std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl; uniform_sampling.setInputCloud (scene); uniform_sampling.setRadiusSearch (scene_ss_); uniform_sampling.filter (*scene_keypoints); std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
下一个阶段在于把3D描述器和每个模型和场景的点进行关联。在这里，我们将计算SHOT描述器，使用SHOTEstiminationOMP
descr_est.setInputCloud (model_keypoints); descr_est.setInputNormals (model_normals); descr_est.setSearchSurface (model); descr_est.compute (*model_descriptors); descr_est.setInputCloud (scene_keypoints); descr_est.setInputNormals (scene_normals); descr_est.setSearchSurface (scene); descr_est.compute (*scene_descriptors);
现在我们需要决定模型描述器和场景间的相关性。为了做到这，这个程序使用了一个KdTreeFLANN。对于每一个和一个场景关键点相关联的描述器，它会以欧拉距离为基础找到最相似的模型描述器，并把这对相似的关键点放到Correspondences这个向量里面。
match_search.setInputCloud (model_descriptors); // For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector. for (size_t i = 0; i < scene_descriptors->size (); ++i) { std::vector<int> neigh_indices (1); std::vector<float> neigh_sqr_dists (1); if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs { continue; } int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists); if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design) { pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]); model_scene_corrs->push_back (corr); } } std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl; // // Actual Clustering
最后的阶段是管道对前面相关的东西进行聚类。
默认的算法是Hough3DGrouping，这是一个以Hough决策为基础的算法。记住这个算法需要与一个Local Reference Frame对每一个作为参数传递的点云关键点。在这个例子里面，我们显示的计算了LRF通过BOARDLocalReferenceFrameEstimation这个预估器。
// Compute (Keypoints) Reference Frames only for Hough // pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ()); pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ()); pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est; rf_est.setFindHoles (true); rf_est.setRadiusSearch (rf_rad_); rf_est.setInputCloud (model_keypoints); rf_est.setInputNormals (model_normals); rf_est.setSearchSurface (model); rf_est.compute (*model_rf); rf_est.setInputCloud (scene_keypoints); rf_est.setInputNormals (scene_normals); rf_est.setSearchSurface (scene); rf_est.compute (*scene_rf); // Clustering pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer; clusterer.setHoughBinSize (cg_size_); clusterer.setHoughThreshold (cg_thresh_); clusterer.setUseInterpolation (true); clusterer.setUseDistanceWeight (false); clusterer.setInputCloud (model_keypoints); clusterer.setInputRf (model_rf); clusterer.setSceneCloud (scene_keypoints); clusterer.setSceneRf (scene_rf); clusterer.setModelSceneCorrespondences (model_scene_corrs); //clusterer.cluster (clustered_corrs); clusterer.recognize (rototranslations, clustered_corrs); } else // Using GeometricConsistency { pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
注意我们并于一定要显示的计算LRF在调用聚类算法前。如果被用来聚类的点云没用相关联的LRF。
Hough3DGrouping算法是可选的，我们还可以使用GeometricConsistencyGrouping这个算法。在这个例子里面LRF计算是不需要的，我们只要简单创建一个算法类的实例，传入相应的参数，并调用相应的recognize这个方法。
gc_clusterer.setGCSize (cg_size_); gc_clusterer.setGCThreshold (cg_thresh_); gc_clusterer.setInputCloud (model_keypoints); gc_clusterer.setSceneCloud (scene_keypoints); gc_clusterer.setModelSceneCorrespondences (model_scene_corrs); //gc_clusterer.cluster (clustered_corrs); gc_clusterer.recognize (rototranslations, clustered_corrs); }
输出和可视化。
std::cout << "\n Instance " << i + 1 << ":" << std::endl; std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl; // Print the rotation matrix and translation vector Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0); Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3); printf ("\n"); printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2)); printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2)); printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2)); printf ("\n"); printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2)); }
程序到时候会显示一个PCLVisualizer这个窗口。如果命令行选项里面-k或者-c被使用了，程序将会显示一个“独立的”经过渲染的点云。
pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ()); pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ()); if (show_correspondences_ || show_keypoints_) { // We are translating the model so that it doesn't end in the middle of the scene representation pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128); viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model"); } if (show_keypoints_) { pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255); viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints"); pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255); viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints"); } for (size_t i = 0; i < rototranslations.size (); ++i) { pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ()); pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]); std::stringstream ss_cloud; ss_cloud << "instance" << i; pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0); viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ()); if (show_correspondences_) { for (size_t j = 0; j < clustered_corrs[i].size (); ++j) { std::stringstream ss_line; ss_line << "correspondence_line" << i << "_" << j; PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query); PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match); // We are drawing a line for each pair of clustered correspondences found between the model and the scene viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ()); } } } while (!viewer.wasStopped ()) { viewer.spinOnce (); } return (0); }
运行程序
./correspondence_grouping milk.pcd milk_cartoon_all_small_clorox.pcd
可选的，你可以指定点云分辨率单元里面的半径
./correspondence_grouping milk.pcd milk_cartoon_all_small_clorox.pcd milk.pcd milk_cartoon_all_small_clorox.pcd -r --model_ss 7.5 --scene_ss 20 --rf_rad 10 --descr_rad 15 --cg_size 10
提示:如果你使用不同的点云并且你不知道如何去设置不同的参数，你可以使用"-r"这个标志还有设置LRF和描述器半径变成分辨率的5 10 15倍。接着你需要小心的调节参数来得到最好的结果。
几秒之后会有如下输出
Model total points: 13704; Selected Keypoints: 732 Scene total points: 307200; Selected Keypoints: 3747 Correspondences found: 1768 Model instances found: 1 Instance 1: Correspondences belonging to this instance: 24 | 0.969 -0.120 0.217 | R = | 0.117 0.993 0.026 | | -0.218 -0.000 0.976 | t = < -0.159, 0.212, -0.042 >
最终提示，如果你的pcl/filters里面没有uniform_sampling.h这个文件你可以从https://github.com/PointCloudLibrary/pcl/tree/master/filters/include/pcl/filters下载uniform_sampling.h和impl里面的uniform_sampling.hpp放到/usr/include/pcl/pcl1.7里面对应的地方。
846
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
