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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:37:18