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
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
00065 if (pcl::console::find_switch (argc, argv, "-h"))
00066 {
00067 showHelp (argv[0]);
00068 exit (0);
00069 }
00070
00071
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
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
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
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
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
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
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
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
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
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
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]))
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)
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
00284
00285 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
00286 std::vector<pcl::Correspondences> clustered_corrs;
00287
00288
00289 if (use_hough_)
00290 {
00291
00292
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
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
00325 clusterer.recognize (rototranslations, clustered_corrs);
00326 }
00327 else
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
00338 gc_clusterer.recognize (rototranslations, clustered_corrs);
00339 }
00340
00341
00342
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
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
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
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
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 }