example1.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 #include <pcl/common/angles.h>
00003 #include <pcl/console/parse.h>
00004 #include <pcl/point_types.h>
00005 #include <pcl/point_cloud.h>
00006 #include <pcl/point_representation.h>
00007 
00008 #include <pcl/io/pcd_io.h>
00009 #include <pcl/console/time.h>
00010 #include <pcl/keypoints/uniform_sampling.h>
00011 #include <pcl/features/normal_3d.h>
00012 #include <pcl/features/fpfh.h>
00013 #include <pcl/registration/correspondence_estimation.h>
00014 #include <pcl/registration/correspondence_estimation_normal_shooting.h>
00015 #include <pcl/registration/correspondence_estimation_backprojection.h>
00016 #include <pcl/registration/correspondence_rejection_median_distance.h>
00017 #include <pcl/registration/correspondence_rejection_surface_normal.h>
00018 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
00019 #include <pcl/registration/default_convergence_criteria.h>
00020 
00021 #include <pcl/visualization/pcl_visualizer.h>
00022 
00023 using namespace std;
00024 using namespace pcl;
00025 using namespace pcl::io;
00026 using namespace pcl::console;
00027 using namespace pcl::registration;
00028 using namespace pcl::visualization;
00029 
00030 typedef PointNormal PointT;
00031 typedef PointCloud<PointT> Cloud;
00032 typedef Cloud::Ptr CloudPtr;
00033 typedef Cloud::ConstPtr CloudConstPtr;
00034 
00035 CloudPtr src, tgt;
00036 
00037 bool rejection = true;
00038 bool visualize = false;
00039 
00040 boost::shared_ptr<PCLVisualizer> vis;
00041 
00043 void
00044 findCorrespondences (const CloudPtr &src,
00045                      const CloudPtr &tgt,
00046                      Correspondences &all_correspondences)
00047 {
00048   //CorrespondenceEstimationNormalShooting<PointT, PointT, PointT> est;
00049   //CorrespondenceEstimation<PointT, PointT> est;
00050   CorrespondenceEstimationBackProjection<PointT, PointT, PointT> est;
00051   est.setInputSource (src);
00052   est.setInputTarget (tgt);
00053   
00054   est.setSourceNormals (src);
00055   est.setTargetNormals (tgt);
00056   est.setKSearch (10);
00057   est.determineCorrespondences (all_correspondences);
00058   //est.determineReciprocalCorrespondences (all_correspondences);
00059 }
00060 
00062 void
00063 rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
00064                           const CloudPtr &src,
00065                           const CloudPtr &tgt,
00066                           Correspondences &remaining_correspondences)
00067 {
00068   CorrespondenceRejectorMedianDistance rej;
00069   rej.setMedianFactor (8.79241104);
00070   rej.setInputCorrespondences (all_correspondences);
00071 
00072   rej.getCorrespondences (remaining_correspondences);
00073   return;
00074   
00075   CorrespondencesPtr remaining_correspondences_temp (new Correspondences);
00076   rej.getCorrespondences (*remaining_correspondences_temp);
00077   PCL_DEBUG ("[rejectBadCorrespondences] Number of correspondences remaining after rejection: %d\n", remaining_correspondences_temp->size ());
00078 
00079   // Reject if the angle between the normals is really off
00080   CorrespondenceRejectorSurfaceNormal rej_normals;
00081   rej_normals.setThreshold (acos (deg2rad (45.0)));
00082   rej_normals.initializeDataContainer<PointT, PointT> ();
00083   rej_normals.setInputCloud<PointT> (src);
00084   rej_normals.setInputNormals<PointT, PointT> (src);
00085   rej_normals.setInputTarget<PointT> (tgt);
00086   rej_normals.setTargetNormals<PointT, PointT> (tgt);
00087   rej_normals.setInputCorrespondences (remaining_correspondences_temp);
00088   rej_normals.getCorrespondences (remaining_correspondences);
00089 }
00090 
00092 void
00093 findTransformation (const CloudPtr &src,
00094                     const CloudPtr &tgt,
00095                     const CorrespondencesPtr &correspondences,
00096                     Eigen::Matrix4d &transform)
00097 {
00098   TransformationEstimationPointToPlaneLLS<PointT, PointT, double> trans_est;
00099   trans_est.estimateRigidTransformation (*src, *tgt, *correspondences, transform);
00100 }
00101 
00103 void
00104 view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
00105 {
00106   if (!visualize || !vis) return;
00107   PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0);
00108   if (!vis->updatePointCloud<PointT> (src, "source"))
00109   {
00110     vis->addPointCloud<PointT> (src, "source");
00111     vis->resetCameraViewpoint ("source");
00112   }
00113   if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target");
00114   vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source");
00115   vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target");
00116   vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source");
00117   pcl::console::TicToc tt;
00118   tt.tic ();
00119   if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1)) 
00120     vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences");
00121   tt.toc_print ();
00122   vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences");
00123   //vis->setShapeRenderingProperties (PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "correspondences");
00124   vis->spin ();
00125 }
00126 
00128 void
00129 icp (const PointCloud<PointT>::Ptr &src, 
00130      const PointCloud<PointT>::Ptr &tgt,
00131      Eigen::Matrix4d &transform)
00132 {
00133   CorrespondencesPtr all_correspondences (new Correspondences), 
00134                      good_correspondences (new Correspondences);
00135 
00136   PointCloud<PointT>::Ptr output (new PointCloud<PointT>);
00137   *output = *src;
00138 
00139   Eigen::Matrix4d final_transform (Eigen::Matrix4d::Identity ());
00140 
00141   int iterations = 0;
00142   DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences);
00143 
00144   // ICP loop
00145   do
00146   {
00147     // Find correspondences
00148     findCorrespondences (output, tgt, *all_correspondences);
00149     PCL_DEBUG ("Number of correspondences found: %d\n", all_correspondences->size ());
00150 
00151     if (rejection)
00152     {
00153       // Reject correspondences
00154       rejectBadCorrespondences (all_correspondences, output, tgt, *good_correspondences);
00155       PCL_DEBUG ("Number of correspondences remaining after rejection: %d\n", good_correspondences->size ());
00156     }
00157     else
00158       *good_correspondences = *all_correspondences;
00159 
00160     // Find transformation
00161     findTransformation (output, tgt, good_correspondences, transform);
00162  
00163     // Obtain the final transformation    
00164     final_transform = transform * final_transform;
00165 
00166     // Transform the data
00167     transformPointCloudWithNormals (*src, *output, final_transform.cast<float> ());
00168 
00169     // Check if convergence has been reached
00170     ++iterations;
00171   
00172     // Visualize the results
00173     view (output, tgt, good_correspondences);
00174   }
00175   while (!converged);
00176   transform = final_transform;
00177 }
00178 
00180 void
00181 saveTransform (const std::string &file, const Eigen::Matrix4d &transform)
00182 {
00183   ofstream ofs;
00184   ofs.open (file.c_str (), ios::trunc | ios::binary);
00185   for (int i = 0; i < 4; ++i)
00186     for (int j = 0; j < 4; ++j)
00187       ofs.write (reinterpret_cast<const char*>(&transform (i, j)), sizeof (double));  
00188   ofs.close ();
00189 }
00190 
00191 /* ---[ */
00192 int
00193 main (int argc, char** argv)
00194 {
00195   // Check whether we want to enable debug mode
00196   bool debug = false;
00197   parse_argument (argc, argv, "-debug", debug);
00198   if (debug)
00199     setVerbosityLevel (L_DEBUG);
00200 
00201   parse_argument (argc, argv, "-rejection", rejection);
00202   parse_argument (argc, argv, "-visualization", visualize);
00203   if (visualize)
00204     vis.reset (new PCLVisualizer ("Registration example"));
00205 
00206   // Parse the command line arguments for .pcd and .transform files
00207   std::vector<int> p_pcd_file_indices, p_tr_file_indices;
00208   p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00209   if (p_pcd_file_indices.size () != 2)
00210   {
00211     print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
00212     print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
00213     return (-1);
00214   }
00215   p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");
00216   if (p_tr_file_indices.size () != 1)
00217   {
00218     print_error ("Need one output transform file to continue.\n");
00219     print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
00220     return (-1);
00221   }
00222   
00223   // Load the files
00224   print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);
00225   src.reset (new PointCloud<PointT>);
00226   tgt.reset (new PointCloud<PointT>);
00227   if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1)
00228   {
00229     print_error ("Error reading the input files!\n");
00230     return (-1);
00231   }
00232 
00233   // Compute the best transformtion
00234   Eigen::Matrix4d transform;
00235   icp (src, tgt, transform);
00236 
00237   saveTransform (argv[p_tr_file_indices[0]], transform);
00238 
00239   cerr.precision (15);
00240   std::cerr << transform << std::endl;
00241 }
00242 /* ]--- */


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