feature_matching.cpp
Go to the documentation of this file.
00001 #include <vector>
00002 #include <string>
00003 #include <sstream>
00004 #include <pcl/io/pcd_io.h>
00005 #include <pcl/registration/transforms.h>
00006 #include <pcl/visualization/pcl_visualizer.h>
00007 #include <pcl/keypoints/sift_keypoint.h>
00008 #include <pcl/keypoints/harris_3d.h>
00009 #include <pcl/ModelCoefficients.h>
00010 #include <pcl/sample_consensus/method_types.h>
00011 #include <pcl/sample_consensus/model_types.h>
00012 #include <pcl/segmentation/sac_segmentation.h>
00013 #include <pcl/search/kdtree.h>
00014 #include <pcl/segmentation/extract_clusters.h>
00015 #include <pcl/features/fpfh_omp.h>
00016 #include <pcl/features/pfh.h>
00017 #include <pcl/features/pfhrgb.h>
00018 #include <pcl/features/3dsc.h>
00019 #include <pcl/features/shot_omp.h>
00020 #include <pcl/kdtree/kdtree_flann.h>
00021 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00022 #include <pcl/registration/transformation_estimation_svd.h>
00023 #include <pcl/registration/icp.h>
00024 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
00025 #include <pcl/common/transforms.h>
00026 #include <pcl/surface/grid_projection.h>
00027 #include <pcl/surface/gp3.h>
00028 #include <pcl/surface/marching_cubes_hoppe.h>
00029 
00030 template<typename FeatureType>
00031 class ICCVTutorial
00032 {
00033   public:
00034     ICCVTutorial (boost::shared_ptr<pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI> > keypoint_detector,
00035                   typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
00036                   boost::shared_ptr<pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal> > surface_reconstructor,
00037                   typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
00038                   typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target);
00039 
00043     void run ();
00044   protected:
00050     void segmentation (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented) const;
00051 
00057     void detectKeypoints (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints) const;
00058 
00065     void extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints, typename pcl::PointCloud<FeatureType>::Ptr features);
00066 
00073     void findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const;
00074 
00078     void filterCorrespondences ();
00079 
00083     void determineInitialTransformation ();
00084 
00088     void determineFinalTransformation ();
00089 
00093     void reconstructSurface ();
00094 
00100     void keyboard_callback (const pcl::visualization::KeyboardEvent& event, void* cookie);
00101 
00102   private:
00103     pcl::visualization::PCLVisualizer visualizer_;
00104     pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_;
00105     pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_;
00106     boost::shared_ptr<pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI> > keypoint_detector_;
00107     typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor_;
00108     boost::shared_ptr<pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal> > surface_reconstructor_;
00109     typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source_;
00110     typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target_;
00111     typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_segmented_;
00112     typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_segmented_;
00113     typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_transformed_;
00114     typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registered_;
00115     typename pcl::PolygonMesh surface_;
00116     typename pcl::PointCloud<FeatureType>::Ptr source_features_;
00117     typename pcl::PointCloud<FeatureType>::Ptr target_features_;
00118     std::vector<int> source2target_;
00119     std::vector<int> target2source_;
00120     pcl::CorrespondencesPtr correspondences_;
00121     Eigen::Matrix4f initial_transformation_matrix_;
00122     Eigen::Matrix4f transformation_matrix_;
00123     bool show_source2target_;
00124     bool show_target2source_;
00125     bool show_correspondences;
00126 };
00127 
00128 template<typename FeatureType>
00129 ICCVTutorial<FeatureType>::ICCVTutorial(boost::shared_ptr<pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI> >keypoint_detector,
00130                                         typename pcl::Feature<pcl::PointXYZRGB, FeatureType>::Ptr feature_extractor,
00131                                         boost::shared_ptr<pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal> > surface_reconstructor,
00132                                         typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source,
00133                                         typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target)
00134 : source_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
00135 , target_keypoints_ (new pcl::PointCloud<pcl::PointXYZI> ())
00136 , keypoint_detector_ (keypoint_detector)
00137 , feature_extractor_ (feature_extractor)
00138 , surface_reconstructor_ (surface_reconstructor)
00139 , source_ (source)
00140 , target_ (target)
00141 , source_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
00142 , target_segmented_ (new pcl::PointCloud<pcl::PointXYZRGB>)
00143 , source_transformed_ (new pcl::PointCloud<pcl::PointXYZRGB>)
00144 , source_registered_ (new pcl::PointCloud<pcl::PointXYZRGB>)
00145 , source_features_ (new pcl::PointCloud<FeatureType>)
00146 , target_features_ (new pcl::PointCloud<FeatureType>)
00147 , correspondences_ (new pcl::Correspondences)
00148 , show_source2target_ (false)
00149 , show_target2source_ (false)
00150 , show_correspondences (false)
00151 {
00152   visualizer_.registerKeyboardCallback(&ICCVTutorial::keyboard_callback, *this, 0);
00153 
00154   segmentation (source_, source_segmented_);
00155   segmentation (target_, target_segmented_);
00156 
00157   detectKeypoints (source_segmented_, source_keypoints_);
00158   detectKeypoints (target_segmented_, target_keypoints_);
00159 
00160   extractDescriptors (source_segmented_, source_keypoints_, source_features_);
00161   extractDescriptors (target_segmented_, target_keypoints_, target_features_);
00162 
00163   findCorrespondences (source_features_, target_features_, source2target_);
00164   findCorrespondences (target_features_, source_features_, target2source_);
00165 
00166   filterCorrespondences ();
00167 
00168   determineInitialTransformation ();
00169   determineFinalTransformation ();
00170 
00171   reconstructSurface ();
00172 }
00173 
00174 template<typename FeatureType>
00175 void ICCVTutorial<FeatureType>::segmentation (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source, typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented) const
00176 {
00177   cout << "segmentation..." << std::flush;
00178   // fit plane and keep points above that plane
00179   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00180   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00181   // Create the segmentation object
00182   pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00183   // Optional
00184   seg.setOptimizeCoefficients (true);
00185   // Mandatory
00186   seg.setModelType (pcl::SACMODEL_PLANE);
00187   seg.setMethodType (pcl::SAC_RANSAC);
00188   seg.setDistanceThreshold (0.02);
00189 
00190   seg.setInputCloud (source);
00191   seg.segment (*inliers, *coefficients);
00192 
00193   pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00194   extract.setInputCloud (source);
00195   extract.setIndices (inliers);
00196   extract.setNegative (true);
00197 
00198   extract.filter (*segmented);
00199   std::vector<int> indices;
00200   pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);
00201   cout << "OK" << endl;
00202 
00203   cout << "clustering..." << std::flush;
00204   // euclidean clustering
00205   typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
00206   tree->setInputCloud (segmented);
00207 
00208   std::vector<pcl::PointIndices> cluster_indices;
00209   pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
00210   clustering.setClusterTolerance (0.02); // 2cm
00211   clustering.setMinClusterSize (1000);
00212   clustering.setMaxClusterSize (250000);
00213   clustering.setSearchMethod (tree);
00214   clustering.setInputCloud(segmented);
00215   clustering.extract (cluster_indices);
00216 
00217   if (cluster_indices.size() > 0)//use largest cluster
00218   {
00219     cout << cluster_indices.size() << " clusters found";
00220     if (cluster_indices.size() > 1)
00221       cout <<" Using largest one...";
00222     cout << endl;
00223     typename pcl::IndicesPtr indices (new std::vector<int>);
00224     *indices = cluster_indices[0].indices;
00225     extract.setInputCloud (segmented);
00226     extract.setIndices (indices);
00227     extract.setNegative (false);
00228 
00229     extract.filter (*segmented);
00230   }
00231 }
00232 
00233 template<typename FeatureType>
00234 void ICCVTutorial<FeatureType>::detectKeypoints (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints) const
00235 {
00236   cout << "keypoint detection..." << std::flush;
00237   keypoint_detector_->setInputCloud(input);
00238   keypoint_detector_->compute(*keypoints);
00239   cout << "OK. keypoints found: " << keypoints->points.size() << endl;
00240 }
00241 
00242 template<typename FeatureType>
00243 void ICCVTutorial<FeatureType>::extractDescriptors (typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, typename pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints, typename pcl::PointCloud<FeatureType>::Ptr features)
00244 {
00245   typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>);
00246   kpts->points.resize(keypoints->points.size());
00247 
00248   pcl::copyPointCloud(*keypoints, *kpts);
00249 
00250   typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType>::Ptr feature_from_normals = boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_);
00251 
00252   feature_extractor_->setSearchSurface(input);
00253   feature_extractor_->setInputCloud(kpts);
00254 
00255   if (feature_from_normals)
00256   //if (boost::dynamic_pointer_cast<typename pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, FeatureType> > (feature_extractor_))
00257   {
00258     cout << "normal estimation..." << std::flush;
00259     typename pcl::PointCloud<pcl::Normal>::Ptr normals (new  pcl::PointCloud<pcl::Normal>);
00260     pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimation;
00261     normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
00262     normal_estimation.setRadiusSearch (0.01);
00263     normal_estimation.setInputCloud (input);
00264     normal_estimation.compute (*normals);
00265     feature_from_normals->setInputNormals(normals);
00266     cout << "OK" << endl;
00267   }
00268 
00269   cout << "descriptor extraction..." << std::flush;
00270   feature_extractor_->compute (*features);
00271   cout << "OK" << endl;
00272 }
00273 
00274 template<typename FeatureType>
00275 void ICCVTutorial<FeatureType>::findCorrespondences (typename pcl::PointCloud<FeatureType>::Ptr source, typename pcl::PointCloud<FeatureType>::Ptr target, std::vector<int>& correspondences) const
00276 {
00277   cout << "correspondence assignment..." << std::flush;
00278   correspondences.resize (source->size());
00279 
00280   // Use a KdTree to search for the nearest matches in feature space
00281   pcl::KdTreeFLANN<FeatureType> descriptor_kdtree;
00282   descriptor_kdtree.setInputCloud (target);
00283 
00284   // Find the index of the best match for each keypoint, and store it in "correspondences_out"
00285   const int k = 1;
00286   std::vector<int> k_indices (k);
00287   std::vector<float> k_squared_distances (k);
00288   for (int i = 0; i < static_cast<int> (source->size ()); ++i)
00289   {
00290     descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
00291     correspondences[i] = k_indices[0];
00292   }
00293   cout << "OK" << endl;
00294 }
00295 
00296 template<typename FeatureType>
00297 void ICCVTutorial<FeatureType>::filterCorrespondences ()
00298 {
00299   cout << "correspondence rejection..." << std::flush;
00300   std::vector<std::pair<unsigned, unsigned> > correspondences;
00301   for (unsigned cIdx = 0; cIdx < source2target_.size (); ++cIdx)
00302     if (target2source_[source2target_[cIdx]] == static_cast<int> (cIdx))
00303       correspondences.push_back(std::make_pair(cIdx, source2target_[cIdx]));
00304 
00305   correspondences_->resize (correspondences.size());
00306   for (unsigned cIdx = 0; cIdx < correspondences.size(); ++cIdx)
00307   {
00308     (*correspondences_)[cIdx].index_query = correspondences[cIdx].first;
00309     (*correspondences_)[cIdx].index_match = correspondences[cIdx].second;
00310   }
00311 
00312   pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZI> rejector;
00313   rejector.setInputSource (source_keypoints_);
00314   rejector.setInputTarget (target_keypoints_);
00315   rejector.setInputCorrespondences(correspondences_);
00316   rejector.getCorrespondences(*correspondences_);
00317   cout << "OK" << endl;
00318 }
00319 
00320 template<typename FeatureType>
00321 void ICCVTutorial<FeatureType>::determineInitialTransformation ()
00322 {
00323   cout << "initial alignment..." << std::flush;
00324   pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr transformation_estimation (new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>);
00325 
00326   transformation_estimation->estimateRigidTransformation (*source_keypoints_, *target_keypoints_, *correspondences_, initial_transformation_matrix_);
00327 
00328   pcl::transformPointCloud(*source_segmented_, *source_transformed_, initial_transformation_matrix_);
00329   cout << "OK" << endl;
00330 }
00331 
00332 template<typename FeatureType>
00333 void ICCVTutorial<FeatureType>::determineFinalTransformation ()
00334 {
00335   cout << "final registration..." << std::flush;
00336   pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr registration (new pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>);
00337   registration->setInputSource (source_transformed_);
00338   //registration->setInputSource (source_segmented_);
00339   registration->setInputTarget (target_segmented_);
00340   registration->setMaxCorrespondenceDistance(0.05);
00341   registration->setRANSACOutlierRejectionThreshold (0.05);
00342   registration->setTransformationEpsilon (0.000001);
00343   registration->setMaximumIterations (1000);
00344   registration->align(*source_registered_);
00345   transformation_matrix_ = registration->getFinalTransformation();
00346   cout << "OK" << endl;
00347 }
00348 
00349 template<typename FeatureType>
00350 void ICCVTutorial<FeatureType>::reconstructSurface ()
00351 {
00352   cout << "surface reconstruction..." << std::flush;
00353   // merge the transformed and the target point cloud
00354   pcl::PointCloud<pcl::PointXYZRGB>::Ptr merged (new pcl::PointCloud<pcl::PointXYZRGB>);
00355   *merged = *source_registered_;
00356   *merged += *target_segmented_;
00357 
00358   // apply grid filtering to reduce amount of points as well as to make them uniform distributed
00359   pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
00360   voxel_grid.setInputCloud(merged);
00361   voxel_grid.setLeafSize (0.002f, 0.002f, 0.002f);
00362   voxel_grid.setDownsampleAllData(true);
00363   voxel_grid.filter(*merged);
00364 
00365   pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr vertices (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00366   pcl::copyPointCloud(*merged, *vertices);
00367 
00368   pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> normal_estimation;
00369   normal_estimation.setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
00370   normal_estimation.setRadiusSearch (0.01);
00371   normal_estimation.setInputCloud (merged);
00372   normal_estimation.compute (*vertices);
00373 
00374   pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
00375   tree->setInputCloud (vertices);
00376 
00377   surface_reconstructor_->setSearchMethod(tree);
00378   surface_reconstructor_->setInputCloud(vertices);
00379   surface_reconstructor_->reconstruct(surface_);
00380   cout << "OK" << endl;
00381 }
00382 
00383 template<typename FeatureType>
00384 void ICCVTutorial<FeatureType>::run()
00385 {
00386   visualizer_.spin ();
00387 }
00388 
00389 template<typename FeatureType>
00390 void ICCVTutorial<FeatureType>::keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
00391 {
00392   if (event.keyUp())
00393   {
00394     switch (event.getKeyCode())
00395     {
00396       case '1':
00397         if (!visualizer_.removePointCloud("source_points"))
00398         {
00399           visualizer_.addPointCloud(source_, "source_points");
00400         }
00401         break;
00402 
00403       case '2':
00404         if (!visualizer_.removePointCloud("target_points"))
00405         {
00406           visualizer_.addPointCloud(target_, "target_points");
00407         }
00408         break;
00409 
00410       case '3':
00411         if (!visualizer_.removePointCloud("source_segmented"))
00412         {
00413           visualizer_.addPointCloud(source_segmented_, "source_segmented");
00414         }
00415         break;
00416 
00417       case '4':
00418         if (!visualizer_.removePointCloud("target_segmented"))
00419         {
00420           visualizer_.addPointCloud(target_segmented_, "target_segmented");
00421         }
00422         break;
00423 
00424       case '5':
00425         if (!visualizer_.removePointCloud("source_keypoints"))
00426         {
00427           pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (source_keypoints_, 0, 0, 255);
00428           //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (source_keypoints_, "intensity");
00429           visualizer_.addPointCloud(source_keypoints_, keypoint_color, "source_keypoints");
00430         }
00431         break;
00432 
00433       case '6':
00434         if (!visualizer_.removePointCloud("target_keypoints"))
00435         {
00436           //pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> keypoint_color (target_keypoints_, "intensity");
00437           pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> keypoint_color (target_keypoints_, 255, 0, 0);
00438           visualizer_.addPointCloud(target_keypoints_, keypoint_color, "target_keypoints");
00439         }
00440         break;
00441 
00442       case '7':
00443         if (!show_source2target_)
00444           visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, source2target_, "source2target");
00445         else
00446           visualizer_.removeCorrespondences("source2target");
00447 
00448         show_source2target_ = !show_source2target_;
00449         break;
00450 
00451       case '8':
00452         if (!show_target2source_)
00453           visualizer_.addCorrespondences<pcl::PointXYZI>(target_keypoints_, source_keypoints_, target2source_, "target2source");
00454         else
00455           visualizer_.removeCorrespondences("target2source");
00456 
00457         show_target2source_ = !show_target2source_;
00458         break;
00459 
00460       case '9':
00461         if (!show_correspondences)
00462           visualizer_.addCorrespondences<pcl::PointXYZI>(source_keypoints_, target_keypoints_, *correspondences_, "correspondences");
00463         else
00464           visualizer_.removeCorrespondences("correspondences");
00465         show_correspondences = !show_correspondences;
00466         break;
00467 
00468       case 'i':
00469       case 'I':
00470         if (!visualizer_.removePointCloud("transformed"))
00471           visualizer_.addPointCloud(source_transformed_, "transformed");
00472         break;
00473 
00474       case 'r':
00475       case 'R':
00476         if (!visualizer_.removePointCloud("registered"))
00477           visualizer_.addPointCloud(source_registered_, "registered");
00478         break;
00479 
00480       case 't':
00481       case 'T':
00482           visualizer_.addPolygonMesh(surface_, "surface");
00483         break;
00484     }
00485   }
00486 }
00487 
00488 int
00489 main (int argc, char ** argv)
00490 {
00491   if (argc < 6)
00492   {
00493     pcl::console::print_info ("Syntax is: %s <source-pcd-file> <target-pcd-file> <keypoint-method> <descriptor-type> <surface-reconstruction-method>\n", argv[0]);
00494     pcl::console::print_info ("available <keypoint-methods>: 1 = Sift3D\n");
00495     pcl::console::print_info ("                              2 = Harris3D\n");
00496     pcl::console::print_info ("                              3 = Tomasi3D\n");
00497     pcl::console::print_info ("                              4 = Noble3D\n");
00498     pcl::console::print_info ("                              5 = Lowe3D\n");
00499     pcl::console::print_info ("                              6 = Curvature3D\n\n");
00500     pcl::console::print_info ("available <descriptor-types>: 1 = FPFH\n");
00501     pcl::console::print_info ("                              2 = SHOTRGB\n");
00502     pcl::console::print_info ("                              3 = PFH\n");
00503     pcl::console::print_info ("                              4 = PFHRGB\n\n");
00504     pcl::console::print_info ("available <surface-methods>:  1 = Greedy Projection\n");
00505     pcl::console::print_info ("                              2 = Marching Cubes\n");
00506 
00507     return (1);
00508   }
00509   pcl::console::print_info ("== MENU ==\n");
00510   pcl::console::print_info ("1 - show/hide source point cloud\n");
00511   pcl::console::print_info ("2 - show/hide target point cloud\n");
00512   pcl::console::print_info ("3 - show/hide segmented source point cloud\n");
00513   pcl::console::print_info ("4 - show/hide segmented target point cloud\n");
00514   pcl::console::print_info ("5 - show/hide source key points\n");
00515   pcl::console::print_info ("6 - show/hide target key points\n");
00516   pcl::console::print_info ("7 - show/hide source2target correspondences\n");
00517   pcl::console::print_info ("8 - show/hide target2source correspondences\n");
00518   pcl::console::print_info ("9 - show/hide consistent correspondences\n");
00519   pcl::console::print_info ("i - show/hide initial alignment\n");
00520   pcl::console::print_info ("r - show/hide final registration\n");
00521   pcl::console::print_info ("t - show/hide triangulation (surface reconstruction)\n");
00522   pcl::console::print_info ("h - show visualizer options\n");
00523   pcl::console::print_info ("q - quit\n");
00524 
00525   pcl::PointCloud<pcl::PointXYZRGB>::Ptr source (new pcl::PointCloud<pcl::PointXYZRGB>);
00526   pcl::io::loadPCDFile (argv[1], *source);
00527 
00528   pcl::PointCloud<pcl::PointXYZRGB>::Ptr target (new pcl::PointCloud<pcl::PointXYZRGB>);
00529   pcl::io::loadPCDFile (argv[2], *target);
00530 
00531   int keypoint_type   = atoi (argv[3]);
00532   int descriptor_type = atoi (argv[4]);
00533   int surface_type    = atoi (argv[5]);
00534 
00535   boost::shared_ptr<pcl::Keypoint<pcl::PointXYZRGB, pcl::PointXYZI> > keypoint_detector;
00536 
00537   if (keypoint_type == 1)
00538   {
00539     pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>* sift3D = new pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI>;
00540     sift3D->setScales (0.01f, 3, 2);
00541     sift3D->setMinimumContrast (0.0);
00542     keypoint_detector.reset (sift3D);
00543   }
00544   else
00545   {
00546     pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>* harris3D = new pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI> (pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
00547     harris3D->setNonMaxSupression (true);
00548     harris3D->setRadius (0.01f);
00549     harris3D->setRadiusSearch (0.01f);
00550     keypoint_detector.reset (harris3D);
00551     switch (keypoint_type)
00552     {
00553       case 2:
00554         harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::HARRIS);
00555       break;
00556 
00557       case 3:
00558         harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::TOMASI);
00559       break;
00560 
00561       case 4:
00562         harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::NOBLE);
00563       break;
00564 
00565       case 5:
00566         harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
00567       break;
00568 
00569       case 6:
00570         harris3D->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::CURVATURE);
00571       break;
00572       default:
00573         pcl::console::print_error("unknown key point detection method %d\n expecting values between 1 and 6", keypoint_type);
00574         exit (1);
00575         break;
00576     }
00577 
00578   }
00579 
00580   boost::shared_ptr<pcl::PCLSurfaceBase<pcl::PointXYZRGBNormal> > surface_reconstruction;
00581 
00582   if (surface_type == 1)
00583   {
00584     pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>* gp3 = new pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal>;
00585 
00586     // Set the maximum distance between connected points (maximum edge length)
00587     gp3->setSearchRadius (0.025);
00588 
00589     // Set typical values for the parameters
00590     gp3->setMu (2.5);
00591     gp3->setMaximumNearestNeighbors (100);
00592     gp3->setMaximumSurfaceAngle(M_PI/4); // 45 degrees
00593     gp3->setMinimumAngle(M_PI/18); // 10 degrees
00594     gp3->setMaximumAngle(2*M_PI/3); // 120 degrees
00595     gp3->setNormalConsistency(false);
00596     surface_reconstruction.reset(gp3);
00597   }
00598   else if (surface_type == 2)
00599   {
00600     pcl::MarchingCubes<pcl::PointXYZRGBNormal>* mc = new pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal>;
00601     mc->setIsoLevel (0.001f);
00602     mc->setGridResolution (50, 50, 50);
00603     surface_reconstruction.reset(mc);
00604   }
00605   else
00606   {
00607     pcl::console::print_error("unknown surface reconstruction method %d\n expecting values between 1 and 2", surface_type);
00608     exit (1);
00609   }
00610 
00611   switch (descriptor_type)
00612   {
00613     case 1:
00614     {
00615       pcl::Feature<pcl::PointXYZRGB, pcl::FPFHSignature33>::Ptr feature_extractor (new pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33>);
00616       feature_extractor->setSearchMethod (pcl::search::Search<pcl::PointXYZRGB>::Ptr (new pcl::search::KdTree<pcl::PointXYZRGB>));
00617       feature_extractor->setRadiusSearch (0.05);
00618       ICCVTutorial<pcl::FPFHSignature33> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
00619       tutorial.run ();
00620     }
00621     break;
00622 
00623     case 2:
00624     {
00625       pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>* shot = new pcl::SHOTColorEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT1344>;
00626       shot->setRadiusSearch (0.04);
00627       pcl::Feature<pcl::PointXYZRGB, pcl::SHOT1344>::Ptr feature_extractor (shot);
00628       ICCVTutorial<pcl::SHOT1344> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
00629       tutorial.run ();
00630     }
00631     break;
00632 
00633     case 3:
00634     {
00635       pcl::Feature<pcl::PointXYZRGB, pcl::PFHSignature125>::Ptr feature_extractor (new pcl::PFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHSignature125>);
00636       feature_extractor->setKSearch(50);
00637       ICCVTutorial<pcl::PFHSignature125> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
00638       tutorial.run ();
00639     }
00640     break;
00641 
00642     case 4:
00643     {
00644       pcl::Feature<pcl::PointXYZRGB, pcl::PFHRGBSignature250>::Ptr feature_extractor (new pcl::PFHRGBEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::PFHRGBSignature250>);
00645       feature_extractor->setKSearch(50);
00646       ICCVTutorial<pcl::PFHRGBSignature250> tutorial (keypoint_detector, feature_extractor, surface_reconstruction, source, target);
00647       tutorial.run ();
00648     }
00649     break;
00650 
00651     default:
00652       pcl::console::print_error("unknown descriptor type %d\n expecting values between 1 and 4", descriptor_type);
00653       exit (1);
00654       break;
00655   }
00656 
00657   return (0);
00658 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:02