template_alignment.cpp
Go to the documentation of this file.
00001 #include <limits>
00002 #include <fstream>
00003 #include <vector>
00004 #include <Eigen/Core>
00005 #include <pcl/point_types.h>
00006 #include <pcl/point_cloud.h>
00007 #include <pcl/io/pcd_io.h>
00008 #include <pcl/kdtree/kdtree_flann.h>
00009 #include <pcl/filters/passthrough.h>
00010 #include <pcl/filters/voxel_grid.h>
00011 #include <pcl/features/normal_3d.h>
00012 #include <pcl/features/fpfh.h>
00013 #include <pcl/registration/ia_ransac.h>
00014 
00015 class FeatureCloud
00016 {
00017   public:
00018     // A bit of shorthand
00019     typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00020     typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
00021     typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
00022     typedef pcl::search::KdTree<pcl::PointXYZ> SearchMethod;
00023 
00024     FeatureCloud () :
00025       search_method_xyz_ (new SearchMethod),
00026       normal_radius_ (0.02f),
00027       feature_radius_ (0.02f)
00028     {}
00029 
00030     ~FeatureCloud () {}
00031 
00032     // Process the given cloud
00033     void
00034     setInputCloud (PointCloud::Ptr xyz)
00035     {
00036       xyz_ = xyz;
00037       processInput ();
00038     }
00039 
00040     // Load and process the cloud in the given PCD file
00041     void
00042     loadInputCloud (const std::string &pcd_file)
00043     {
00044       xyz_ = PointCloud::Ptr (new PointCloud);
00045       pcl::io::loadPCDFile (pcd_file, *xyz_);
00046       processInput ();
00047     }
00048 
00049     // Get a pointer to the cloud 3D points
00050     PointCloud::Ptr
00051     getPointCloud () const
00052     {
00053       return (xyz_);
00054     }
00055 
00056     // Get a pointer to the cloud of 3D surface normals
00057     SurfaceNormals::Ptr
00058     getSurfaceNormals () const
00059     {
00060       return (normals_);
00061     }
00062 
00063     // Get a pointer to the cloud of feature descriptors
00064     LocalFeatures::Ptr
00065     getLocalFeatures () const
00066     {
00067       return (features_);
00068     }
00069 
00070   protected:
00071     // Compute the surface normals and local features
00072     void
00073     processInput ()
00074     {
00075       computeSurfaceNormals ();
00076       computeLocalFeatures ();
00077     }
00078 
00079     // Compute the surface normals
00080     void
00081     computeSurfaceNormals ()
00082     {
00083       normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
00084 
00085       pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
00086       norm_est.setInputCloud (xyz_);
00087       norm_est.setSearchMethod (search_method_xyz_);
00088       norm_est.setRadiusSearch (normal_radius_);
00089       norm_est.compute (*normals_);
00090     }
00091 
00092     // Compute the local feature descriptors
00093     void
00094     computeLocalFeatures ()
00095     {
00096       features_ = LocalFeatures::Ptr (new LocalFeatures);
00097 
00098       pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
00099       fpfh_est.setInputCloud (xyz_);
00100       fpfh_est.setInputNormals (normals_);
00101       fpfh_est.setSearchMethod (search_method_xyz_);
00102       fpfh_est.setRadiusSearch (feature_radius_);
00103       fpfh_est.compute (*features_);
00104     }
00105 
00106   private:
00107     // Point cloud data
00108     PointCloud::Ptr xyz_;
00109     SurfaceNormals::Ptr normals_;
00110     LocalFeatures::Ptr features_;
00111     SearchMethod::Ptr search_method_xyz_;
00112 
00113     // Parameters
00114     float normal_radius_;
00115     float feature_radius_;
00116 };
00117 
00118 class TemplateAlignment
00119 {
00120   public:
00121 
00122     // A struct for storing alignment results
00123     struct Result
00124     {
00125       float fitness_score;
00126       Eigen::Matrix4f final_transformation;
00127       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00128     };
00129 
00130     TemplateAlignment () :
00131       min_sample_distance_ (0.05f),
00132       max_correspondence_distance_ (0.01f*0.01f),
00133       nr_iterations_ (500)
00134     {
00135       // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
00136       sac_ia_.setMinSampleDistance (min_sample_distance_);
00137       sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
00138       sac_ia_.setMaximumIterations (nr_iterations_);
00139     }
00140 
00141     ~TemplateAlignment () {}
00142 
00143     // Set the given cloud as the target to which the templates will be aligned
00144     void
00145     setTargetCloud (FeatureCloud &target_cloud)
00146     {
00147       target_ = target_cloud;
00148       sac_ia_.setInputTarget (target_cloud.getPointCloud ());
00149       sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
00150     }
00151 
00152     // Add the given cloud to the list of template clouds
00153     void
00154     addTemplateCloud (FeatureCloud &template_cloud)
00155     {
00156       templates_.push_back (template_cloud);
00157     }
00158 
00159     // Align the given template cloud to the target specified by setTargetCloud ()
00160     void
00161     align (FeatureCloud &template_cloud, TemplateAlignment::Result &result)
00162     {
00163       sac_ia_.setInputCloud (template_cloud.getPointCloud ());
00164       sac_ia_.setSourceFeatures (template_cloud.getLocalFeatures ());
00165 
00166       pcl::PointCloud<pcl::PointXYZ> registration_output;
00167       sac_ia_.align (registration_output);
00168 
00169       result.fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);
00170       result.final_transformation = sac_ia_.getFinalTransformation ();
00171     }
00172 
00173     // Align all of template clouds set by addTemplateCloud to the target specified by setTargetCloud ()
00174     void
00175     alignAll (std::vector<TemplateAlignment::Result, Eigen::aligned_allocator<Result> > &results)
00176     {
00177       results.resize (templates_.size ());
00178       for (size_t i = 0; i < templates_.size (); ++i)
00179       {
00180         align (templates_[i], results[i]);
00181       }
00182     }
00183 
00184     // Align all of template clouds to the target cloud to find the one with best alignment score
00185     int
00186     findBestAlignment (TemplateAlignment::Result &result)
00187     {
00188       // Align all of the templates to the target cloud
00189       std::vector<Result, Eigen::aligned_allocator<Result> > results;
00190       alignAll (results);
00191 
00192       // Find the template with the best (lowest) fitness score
00193       float lowest_score = std::numeric_limits<float>::infinity ();
00194       int best_template = 0;
00195       for (size_t i = 0; i < results.size (); ++i)
00196       {
00197         const Result &r = results[i];
00198         if (r.fitness_score < lowest_score)
00199         {
00200           lowest_score = r.fitness_score;
00201           best_template = (int) i;
00202         }
00203       }
00204 
00205       // Output the best alignment
00206       result = results[best_template];
00207       return (best_template);
00208     }
00209 
00210   private:
00211     // A list of template clouds and the target to which they will be aligned
00212     std::vector<FeatureCloud> templates_;
00213     FeatureCloud target_;
00214 
00215     // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
00216     pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
00217     float min_sample_distance_;
00218     float max_correspondence_distance_;
00219     int nr_iterations_;
00220 };
00221 
00222 // Align a collection of object templates to a sample point cloud
00223 int
00224 main (int argc, char **argv)
00225 {
00226   if (argc < 3)
00227   {
00228     printf ("No target PCD file given!\n");
00229     return (-1);
00230   }
00231 
00232   // Load the object templates specified in the object_templates.txt file
00233   std::vector<FeatureCloud> object_templates;
00234   std::ifstream input_stream (argv[1]);
00235   object_templates.resize (0);
00236   std::string pcd_filename;
00237   while (input_stream.good ())
00238   {
00239     std::getline (input_stream, pcd_filename);
00240     if (pcd_filename.empty () || pcd_filename.at (0) == '#') // Skip blank lines or comments
00241       continue;
00242 
00243     FeatureCloud template_cloud;
00244     template_cloud.loadInputCloud (pcd_filename);
00245     object_templates.push_back (template_cloud);
00246   }
00247   input_stream.close ();
00248 
00249   // Load the target cloud PCD file
00250   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00251   pcl::io::loadPCDFile (argv[2], *cloud);
00252 
00253   // Preprocess the cloud by...
00254   // ...removing distant points
00255   const float depth_limit = 1.0;
00256   pcl::PassThrough<pcl::PointXYZ> pass;
00257   pass.setInputCloud (cloud);
00258   pass.setFilterFieldName ("z");
00259   pass.setFilterLimits (0, depth_limit);
00260   pass.filter (*cloud);
00261 
00262   // ... and downsampling the point cloud
00263   const float voxel_grid_size = 0.005f;
00264   pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
00265   vox_grid.setInputCloud (cloud);
00266   vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
00267   //vox_grid.filter (*cloud); // Please see this http://www.pcl-developers.org/Possible-problem-in-new-VoxelGrid-implementation-from-PCL-1-5-0-td5490361.html
00268   pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud (new pcl::PointCloud<pcl::PointXYZ>); 
00269   vox_grid.filter (*tempCloud);
00270   cloud = tempCloud; 
00271 
00272   // Assign to the target FeatureCloud
00273   FeatureCloud target_cloud;
00274   target_cloud.setInputCloud (cloud);
00275 
00276   // Set the TemplateAlignment inputs
00277   TemplateAlignment template_align;
00278   for (size_t i = 0; i < object_templates.size (); ++i)
00279   {
00280     template_align.addTemplateCloud (object_templates[i]);
00281   }
00282   template_align.setTargetCloud (target_cloud);
00283 
00284   // Find the best template alignment
00285   TemplateAlignment::Result best_alignment;
00286   int best_index = template_align.findBestAlignment (best_alignment);
00287   const FeatureCloud &best_template = object_templates[best_index];
00288 
00289   // Print the alignment fitness score (values less than 0.00002 are good)
00290   printf ("Best fitness score: %f\n", best_alignment.fitness_score);
00291 
00292   // Print the rotation matrix and translation vector
00293   Eigen::Matrix3f rotation = best_alignment.final_transformation.block<3,3>(0, 0);
00294   Eigen::Vector3f translation = best_alignment.final_transformation.block<3,1>(0, 3);
00295 
00296   printf ("\n");
00297   printf ("    | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
00298   printf ("R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
00299   printf ("    | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
00300   printf ("\n");
00301   printf ("t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
00302 
00303   // Save the aligned template for visualization
00304   pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
00305   pcl::transformPointCloud (*best_template.getPointCloud (), transformed_cloud, best_alignment.final_transformation);
00306   pcl::io::savePCDFileBinary ("output.pcd", transformed_cloud);
00307 
00308   return (0);
00309 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:34:20