correspondence_grouping.cpp
Go to the documentation of this file.
00001 #include <pcl/io/pcd_io.h>
00002 #include <pcl/point_cloud.h>
00003 #include <pcl/correspondence.h>
00004 #include <pcl/features/normal_3d_omp.h>
00005 #include <pcl/features/shot_omp.h>
00006 #include <pcl/features/board.h>
00007 #include <pcl/keypoints/uniform_sampling.h>
00008 #include <pcl/recognition/cg/hough_3d.h>
00009 #include <pcl/recognition/cg/geometric_consistency.h>
00010 #include <pcl/visualization/pcl_visualizer.h>
00011 #include <pcl/kdtree/kdtree_flann.h>
00012 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00013 #include <pcl/common/transforms.h>
00014 #include <pcl/console/parse.h>
00015 
00016 typedef pcl::PointXYZRGBA PointType;
00017 typedef pcl::Normal NormalType;
00018 typedef pcl::ReferenceFrame RFType;
00019 typedef pcl::SHOT352 DescriptorType;
00020 
00021 std::string model_filename_;
00022 std::string scene_filename_;
00023 
00024 //Algorithm params
00025 bool show_keypoints_ (false);
00026 bool show_correspondences_ (false);
00027 bool use_cloud_resolution_ (false);
00028 bool use_hough_ (true);
00029 float model_ss_ (0.01f);
00030 float scene_ss_ (0.03f);
00031 float rf_rad_ (0.015f);
00032 float descr_rad_ (0.02f);
00033 float cg_size_ (0.01f);
00034 float cg_thresh_ (5.0f);
00035 
00036 void
00037 showHelp (char *filename)
00038 {
00039   std::cout << std::endl;
00040   std::cout << "***************************************************************************" << std::endl;
00041   std::cout << "*                                                                         *" << std::endl;
00042   std::cout << "*             Correspondence Grouping Tutorial - Usage Guide              *" << std::endl;
00043   std::cout << "*                                                                         *" << std::endl;
00044   std::cout << "***************************************************************************" << std::endl << std::endl;
00045   std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
00046   std::cout << "Options:" << std::endl;
00047   std::cout << "     -h:                     Show this help." << std::endl;
00048   std::cout << "     -k:                     Show used keypoints." << std::endl;
00049   std::cout << "     -c:                     Show used correspondences." << std::endl;
00050   std::cout << "     -r:                     Compute the model cloud resolution and multiply" << std::endl;
00051   std::cout << "                             each radius given by that value." << std::endl;
00052   std::cout << "     --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;
00053   std::cout << "     --model_ss val:         Model uniform sampling radius (default 0.01)" << std::endl;
00054   std::cout << "     --scene_ss val:         Scene uniform sampling radius (default 0.03)" << std::endl;
00055   std::cout << "     --rf_rad val:           Reference frame radius (default 0.015)" << std::endl;
00056   std::cout << "     --descr_rad val:        Descriptor radius (default 0.02)" << std::endl;
00057   std::cout << "     --cg_size val:          Cluster size (default 0.01)" << std::endl;
00058   std::cout << "     --cg_thresh val:        Clustering threshold (default 5)" << std::endl << std::endl;
00059 }
00060 
00061 void
00062 parseCommandLine (int argc, char *argv[])
00063 {
00064   //Show help
00065   if (pcl::console::find_switch (argc, argv, "-h"))
00066   {
00067     showHelp (argv[0]);
00068     exit (0);
00069   }
00070 
00071   //Model & scene filenames
00072   std::vector<int> filenames;
00073   filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
00074   if (filenames.size () != 2)
00075   {
00076     std::cout << "Filenames missing.\n";
00077     showHelp (argv[0]);
00078     exit (-1);
00079   }
00080 
00081   model_filename_ = argv[filenames[0]];
00082   scene_filename_ = argv[filenames[1]];
00083 
00084   //Program behavior
00085   if (pcl::console::find_switch (argc, argv, "-k"))
00086   {
00087     show_keypoints_ = true;
00088   }
00089   if (pcl::console::find_switch (argc, argv, "-c"))
00090   {
00091     show_correspondences_ = true;
00092   }
00093   if (pcl::console::find_switch (argc, argv, "-r"))
00094   {
00095     use_cloud_resolution_ = true;
00096   }
00097 
00098   std::string used_algorithm;
00099   if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
00100   {
00101     if (used_algorithm.compare ("Hough") == 0)
00102     {
00103       use_hough_ = true;
00104     }else if (used_algorithm.compare ("GC") == 0)
00105     {
00106       use_hough_ = false;
00107     }
00108     else
00109     {
00110       std::cout << "Wrong algorithm name.\n";
00111       showHelp (argv[0]);
00112       exit (-1);
00113     }
00114   }
00115 
00116   //General parameters
00117   pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
00118   pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
00119   pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
00120   pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
00121   pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
00122   pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
00123 }
00124 
00125 double
00126 computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
00127 {
00128   double res = 0.0;
00129   int n_points = 0;
00130   int nres;
00131   std::vector<int> indices (2);
00132   std::vector<float> sqr_distances (2);
00133   pcl::search::KdTree<PointType> tree;
00134   tree.setInputCloud (cloud);
00135 
00136   for (size_t i = 0; i < cloud->size (); ++i)
00137   {
00138     if (! pcl_isfinite ((*cloud)[i].x))
00139     {
00140       continue;
00141     }
00142     //Considering the second neighbor since the first is the point itself.
00143     nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
00144     if (nres == 2)
00145     {
00146       res += sqrt (sqr_distances[1]);
00147       ++n_points;
00148     }
00149   }
00150   if (n_points != 0)
00151   {
00152     res /= n_points;
00153   }
00154   return res;
00155 }
00156 
00157 int
00158 main (int argc, char *argv[])
00159 {
00160   parseCommandLine (argc, argv);
00161 
00162   pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
00163   pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());
00164   pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());
00165   pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());
00166   pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());
00167   pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());
00168   pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
00169   pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());
00170 
00171   //
00172   //  Load clouds
00173   //
00174   if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
00175   {
00176     std::cout << "Error loading model cloud." << std::endl;
00177     showHelp (argv[0]);
00178     return (-1);
00179   }
00180   if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
00181   {
00182     std::cout << "Error loading scene cloud." << std::endl;
00183     showHelp (argv[0]);
00184     return (-1);
00185   }
00186 
00187   //
00188   //  Set up resolution invariance
00189   //
00190   if (use_cloud_resolution_)
00191   {
00192     float resolution = static_cast<float> (computeCloudResolution (model));
00193     if (resolution != 0.0f)
00194     {
00195       model_ss_   *= resolution;
00196       scene_ss_   *= resolution;
00197       rf_rad_     *= resolution;
00198       descr_rad_  *= resolution;
00199       cg_size_    *= resolution;
00200     }
00201 
00202     std::cout << "Model resolution:       " << resolution << std::endl;
00203     std::cout << "Model sampling size:    " << model_ss_ << std::endl;
00204     std::cout << "Scene sampling size:    " << scene_ss_ << std::endl;
00205     std::cout << "LRF support radius:     " << rf_rad_ << std::endl;
00206     std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
00207     std::cout << "Clustering bin size:    " << cg_size_ << std::endl << std::endl;
00208   }
00209 
00210   //
00211   //  Compute Normals
00212   //
00213   pcl::NormalEstimationOMP<PointType, NormalType> norm_est;
00214   norm_est.setKSearch (10);
00215   norm_est.setInputCloud (model);
00216   norm_est.compute (*model_normals);
00217 
00218   norm_est.setInputCloud (scene);
00219   norm_est.compute (*scene_normals);
00220 
00221   //
00222   //  Downsample Clouds to Extract keypoints
00223   //
00224   pcl::PointCloud<int> sampled_indices;
00225 
00226   pcl::UniformSampling<PointType> uniform_sampling;
00227   uniform_sampling.setInputCloud (model);
00228   uniform_sampling.setRadiusSearch (model_ss_);
00229   uniform_sampling.compute (sampled_indices);
00230   pcl::copyPointCloud (*model, sampled_indices.points, *model_keypoints);
00231   std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;
00232 
00233   uniform_sampling.setInputCloud (scene);
00234   uniform_sampling.setRadiusSearch (scene_ss_);
00235   uniform_sampling.compute (sampled_indices);
00236   pcl::copyPointCloud (*scene, sampled_indices.points, *scene_keypoints);
00237   std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;
00238 
00239 
00240   //
00241   //  Compute Descriptor for keypoints
00242   //
00243   pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
00244   descr_est.setRadiusSearch (descr_rad_);
00245 
00246   descr_est.setInputCloud (model_keypoints);
00247   descr_est.setInputNormals (model_normals);
00248   descr_est.setSearchSurface (model);
00249   descr_est.compute (*model_descriptors);
00250 
00251   descr_est.setInputCloud (scene_keypoints);
00252   descr_est.setInputNormals (scene_normals);
00253   descr_est.setSearchSurface (scene);
00254   descr_est.compute (*scene_descriptors);
00255 
00256   //
00257   //  Find Model-Scene Correspondences with KdTree
00258   //
00259   pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());
00260 
00261   pcl::KdTreeFLANN<DescriptorType> match_search;
00262   match_search.setInputCloud (model_descriptors);
00263 
00264   //  For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
00265   for (size_t i = 0; i < scene_descriptors->size (); ++i)
00266   {
00267     std::vector<int> neigh_indices (1);
00268     std::vector<float> neigh_sqr_dists (1);
00269     if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
00270     {
00271       continue;
00272     }
00273     int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);
00274     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)
00275     {
00276       pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
00277       model_scene_corrs->push_back (corr);
00278     }
00279   }
00280   std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;
00281 
00282   //
00283   //  Actual Clustering
00284   //
00285   std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
00286   std::vector<pcl::Correspondences> clustered_corrs;
00287 
00288   //  Using Hough3D
00289   if (use_hough_)
00290   {
00291     //
00292     //  Compute (Keypoints) Reference Frames only for Hough
00293     //
00294     pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
00295     pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());
00296 
00297     pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
00298     rf_est.setFindHoles (true);
00299     rf_est.setRadiusSearch (rf_rad_);
00300 
00301     rf_est.setInputCloud (model_keypoints);
00302     rf_est.setInputNormals (model_normals);
00303     rf_est.setSearchSurface (model);
00304     rf_est.compute (*model_rf);
00305 
00306     rf_est.setInputCloud (scene_keypoints);
00307     rf_est.setInputNormals (scene_normals);
00308     rf_est.setSearchSurface (scene);
00309     rf_est.compute (*scene_rf);
00310 
00311     //  Clustering
00312     pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
00313     clusterer.setHoughBinSize (cg_size_);
00314     clusterer.setHoughThreshold (cg_thresh_);
00315     clusterer.setUseInterpolation (true);
00316     clusterer.setUseDistanceWeight (false);
00317 
00318     clusterer.setInputCloud (model_keypoints);
00319     clusterer.setInputRf (model_rf);
00320     clusterer.setSceneCloud (scene_keypoints);
00321     clusterer.setSceneRf (scene_rf);
00322     clusterer.setModelSceneCorrespondences (model_scene_corrs);
00323 
00324     //clusterer.cluster (clustered_corrs);
00325     clusterer.recognize (rototranslations, clustered_corrs);
00326   }
00327   else // Using GeometricConsistency
00328   {
00329     pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
00330     gc_clusterer.setGCSize (cg_size_);
00331     gc_clusterer.setGCThreshold (cg_thresh_);
00332 
00333     gc_clusterer.setInputCloud (model_keypoints);
00334     gc_clusterer.setSceneCloud (scene_keypoints);
00335     gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);
00336 
00337     //gc_clusterer.cluster (clustered_corrs);
00338     gc_clusterer.recognize (rototranslations, clustered_corrs);
00339   }
00340 
00341   //
00342   //  Output results
00343   //
00344   std::cout << "Model instances found: " << rototranslations.size () << std::endl;
00345   for (size_t i = 0; i < rototranslations.size (); ++i)
00346   {
00347     std::cout << "\n    Instance " << i + 1 << ":" << std::endl;
00348     std::cout << "        Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;
00349 
00350     // Print the rotation matrix and translation vector
00351     Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
00352     Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);
00353 
00354     printf ("\n");
00355     printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
00356     printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
00357     printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
00358     printf ("\n");
00359     printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
00360   }
00361 
00362   //
00363   //  Visualization
00364   //
00365   pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping");
00366   viewer.addPointCloud (scene, "scene_cloud");
00367 
00368   pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
00369   pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());
00370 
00371   if (show_correspondences_ || show_keypoints_)
00372   {
00373     //  We are translating the model so that it doesn't end in the middle of the scene representation
00374     pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
00375     pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0));
00376 
00377     pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128);
00378     viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
00379   }
00380 
00381   if (show_keypoints_)
00382   {
00383     pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
00384     viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
00385     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");
00386 
00387     pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
00388     viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
00389     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
00390   }
00391 
00392   for (size_t i = 0; i < rototranslations.size (); ++i)
00393   {
00394     pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
00395     pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);
00396 
00397     std::stringstream ss_cloud;
00398     ss_cloud << "instance" << i;
00399 
00400     pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
00401     viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());
00402 
00403     if (show_correspondences_)
00404     {
00405       for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
00406       {
00407         std::stringstream ss_line;
00408         ss_line << "correspondence_line" << i << "_" << j;
00409         PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);
00410         PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);
00411 
00412         //  We are drawing a line for each pair of clustered correspondences found between the model and the scene
00413         viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
00414       }
00415     }
00416   }
00417 
00418   while (!viewer.wasStopped ())
00419   {
00420     viewer.spinOnce ();
00421   }
00422 
00423   return (0);
00424 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:01