example2.cpp
Go to the documentation of this file.
00001 #include <pcl/console/parse.h>
00002 #include <pcl/point_types.h>
00003 #include <pcl/point_cloud.h>
00004 #include <pcl/point_representation.h>
00005 
00006 #include <pcl/io/pcd_io.h>
00007 #include <pcl/conversions.h>
00008 #include <pcl/keypoints/uniform_sampling.h>
00009 #include <pcl/features/normal_3d.h>
00010 #include <pcl/features/fpfh.h>
00011 #include <pcl/registration/correspondence_estimation.h>
00012 #include <pcl/registration/correspondence_rejection_distance.h>
00013 #include <pcl/registration/transformation_estimation_svd.h>
00014 
00015 using namespace std;
00016 using namespace pcl;
00017 using namespace pcl::io;
00018 using namespace pcl::console;
00019 using namespace pcl::registration;
00020 PointCloud<PointXYZ>::Ptr src, tgt;
00021 
00023 void
00024 estimateKeypoints (const PointCloud<PointXYZ>::Ptr &src, 
00025                    const PointCloud<PointXYZ>::Ptr &tgt,
00026                    PointCloud<PointXYZ> &keypoints_src,
00027                    PointCloud<PointXYZ> &keypoints_tgt)
00028 {
00029   PointCloud<int> keypoints_src_idx, keypoints_tgt_idx;
00030   // Get an uniform grid of keypoints
00031   UniformSampling<PointXYZ> uniform;
00032   uniform.setRadiusSearch (1);  // 1m
00033 
00034   uniform.setInputCloud (src);
00035   uniform.compute (keypoints_src_idx);
00036   copyPointCloud<PointXYZ, PointXYZ> (*src, keypoints_src_idx.points, keypoints_src);
00037 
00038   uniform.setInputCloud (tgt);
00039   uniform.compute (keypoints_tgt_idx);
00040   copyPointCloud<PointXYZ, PointXYZ> (*tgt, keypoints_tgt_idx.points, keypoints_tgt);
00041 
00042   // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
00043   // pcd_viewer source_pcd keypoints_src.pcd -ps 1 -ps 10
00044   savePCDFileBinary ("keypoints_src.pcd", keypoints_src);
00045   savePCDFileBinary ("keypoints_tgt.pcd", keypoints_tgt);
00046 }
00047 
00049 void
00050 estimateNormals (const PointCloud<PointXYZ>::Ptr &src, 
00051                  const PointCloud<PointXYZ>::Ptr &tgt,
00052                  PointCloud<Normal> &normals_src,
00053                  PointCloud<Normal> &normals_tgt)
00054 {
00055   NormalEstimation<PointXYZ, Normal> normal_est;
00056   normal_est.setInputCloud (src);
00057   normal_est.setRadiusSearch (0.5);  // 50cm
00058   normal_est.compute (normals_src);
00059 
00060   normal_est.setInputCloud (tgt);
00061   normal_est.compute (normals_tgt);
00062 
00063   // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
00064   // pcd_viewer normals_src.pcd 
00065   PointCloud<PointNormal> s, t;
00066   copyPointCloud<PointXYZ, PointNormal> (*src, s);
00067   copyPointCloud<Normal, PointNormal> (normals_src, s);
00068   copyPointCloud<PointXYZ, PointNormal> (*tgt, t);
00069   copyPointCloud<Normal, PointNormal> (normals_tgt, t);
00070   savePCDFileBinary ("normals_src.pcd", s);
00071   savePCDFileBinary ("normals_tgt.pcd", t);
00072 }
00073 
00075 void
00076 estimateFPFH (const PointCloud<PointXYZ>::Ptr &src, 
00077               const PointCloud<PointXYZ>::Ptr &tgt,
00078               const PointCloud<Normal>::Ptr &normals_src,
00079               const PointCloud<Normal>::Ptr &normals_tgt,
00080               const PointCloud<PointXYZ>::Ptr &keypoints_src,
00081               const PointCloud<PointXYZ>::Ptr &keypoints_tgt,
00082               PointCloud<FPFHSignature33> &fpfhs_src,
00083               PointCloud<FPFHSignature33> &fpfhs_tgt)
00084 {
00085   FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh_est;
00086   fpfh_est.setInputCloud (keypoints_src);
00087   fpfh_est.setInputNormals (normals_src);
00088   fpfh_est.setRadiusSearch (1); // 1m
00089   fpfh_est.setSearchSurface (src);
00090   fpfh_est.compute (fpfhs_src);
00091 
00092   fpfh_est.setInputCloud (keypoints_tgt);
00093   fpfh_est.setInputNormals (normals_tgt);
00094   fpfh_est.setSearchSurface (tgt);
00095   fpfh_est.compute (fpfhs_tgt);
00096 
00097   // For debugging purposes only: uncomment the lines below and use pcd_viewer to view the results, i.e.:
00098   // pcd_viewer fpfhs_src.pcd 
00099   PCLPointCloud2 s, t, out;
00100   toPCLPointCloud2 (*keypoints_src, s); toPCLPointCloud2 (fpfhs_src, t); concatenateFields (s, t, out);
00101   savePCDFile ("fpfhs_src.pcd", out);
00102   toPCLPointCloud2 (*keypoints_tgt, s); toPCLPointCloud2 (fpfhs_tgt, t); concatenateFields (s, t, out);
00103   savePCDFile ("fpfhs_tgt.pcd", out);
00104 }
00105 
00107 void
00108 findCorrespondences (const PointCloud<FPFHSignature33>::Ptr &fpfhs_src,
00109                      const PointCloud<FPFHSignature33>::Ptr &fpfhs_tgt,
00110                      Correspondences &all_correspondences)
00111 {
00112   CorrespondenceEstimation<FPFHSignature33, FPFHSignature33> est;
00113   est.setInputCloud (fpfhs_src);
00114   est.setInputTarget (fpfhs_tgt);
00115   est.determineReciprocalCorrespondences (all_correspondences);
00116 }
00117 
00119 void
00120 rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
00121                           const PointCloud<PointXYZ>::Ptr &keypoints_src,
00122                           const PointCloud<PointXYZ>::Ptr &keypoints_tgt,
00123                           Correspondences &remaining_correspondences)
00124 {
00125   CorrespondenceRejectorDistance rej;
00126   rej.setInputCloud<PointXYZ> (keypoints_src);
00127   rej.setInputTarget<PointXYZ> (keypoints_tgt);
00128   rej.setMaximumDistance (1);    // 1m
00129   rej.setInputCorrespondences (all_correspondences);
00130   rej.getCorrespondences (remaining_correspondences);
00131 }
00132 
00133 
00135 void
00136 computeTransformation (const PointCloud<PointXYZ>::Ptr &src, 
00137                        const PointCloud<PointXYZ>::Ptr &tgt,
00138                        Eigen::Matrix4f &transform)
00139 {
00140   // Get an uniform grid of keypoints
00141   PointCloud<PointXYZ>::Ptr keypoints_src (new PointCloud<PointXYZ>), 
00142                             keypoints_tgt (new PointCloud<PointXYZ>);
00143 
00144   estimateKeypoints (src, tgt, *keypoints_src, *keypoints_tgt);
00145   print_info ("Found %zu and %zu keypoints for the source and target datasets.\n", keypoints_src->points.size (), keypoints_tgt->points.size ());
00146 
00147   // Compute normals for all points keypoint
00148   PointCloud<Normal>::Ptr normals_src (new PointCloud<Normal>), 
00149                           normals_tgt (new PointCloud<Normal>);
00150   estimateNormals (src, tgt, *normals_src, *normals_tgt);
00151   print_info ("Estimated %zu and %zu normals for the source and target datasets.\n", normals_src->points.size (), normals_tgt->points.size ());
00152 
00153   // Compute FPFH features at each keypoint
00154   PointCloud<FPFHSignature33>::Ptr fpfhs_src (new PointCloud<FPFHSignature33>), 
00155                                    fpfhs_tgt (new PointCloud<FPFHSignature33>);
00156   estimateFPFH (src, tgt, normals_src, normals_tgt, keypoints_src, keypoints_tgt, *fpfhs_src, *fpfhs_tgt);
00157 
00158   // Copy the data and save it to disk
00159 /*  PointCloud<PointNormal> s, t;
00160   copyPointCloud<PointXYZ, PointNormal> (*keypoints_src, s);
00161   copyPointCloud<Normal, PointNormal> (normals_src, s);
00162   copyPointCloud<PointXYZ, PointNormal> (*keypoints_tgt, t);
00163   copyPointCloud<Normal, PointNormal> (normals_tgt, t);*/
00164 
00165   // Find correspondences between keypoints in FPFH space
00166   CorrespondencesPtr all_correspondences (new Correspondences), 
00167                      good_correspondences (new Correspondences);
00168   findCorrespondences (fpfhs_src, fpfhs_tgt, *all_correspondences);
00169 
00170   // Reject correspondences based on their XYZ distance
00171   rejectBadCorrespondences (all_correspondences, keypoints_src, keypoints_tgt, *good_correspondences);
00172 
00173   for (int i = 0; i < good_correspondences->size (); ++i)
00174     std::cerr << good_correspondences->at (i) << std::endl;
00175   // Obtain the best transformation between the two sets of keypoints given the remaining correspondences
00176   TransformationEstimationSVD<PointXYZ, PointXYZ> trans_est;
00177   trans_est.estimateRigidTransformation (*keypoints_src, *keypoints_tgt, *good_correspondences, transform);
00178 }
00179 
00180 /* ---[ */
00181 int
00182 main (int argc, char** argv)
00183 {
00184   // Parse the command line arguments for .pcd files
00185   std::vector<int> p_file_indices;
00186   p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00187   if (p_file_indices.size () != 2)
00188   {
00189     print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
00190     print_error ("Example: %s source.pcd target.pcd\n", argv[0]);
00191     return (-1);
00192   }
00193 
00194   // Load the files
00195   print_info ("Loading %s as source and %s as target...\n", argv[p_file_indices[0]], argv[p_file_indices[1]]);
00196   src.reset (new PointCloud<PointXYZ>);
00197   tgt.reset (new PointCloud<PointXYZ>);
00198   if (loadPCDFile (argv[p_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_file_indices[1]], *tgt) == -1)
00199   {
00200     print_error ("Error reading the input files!\n");
00201     return (-1);
00202   }
00203 
00204   // Compute the best transformtion
00205   Eigen::Matrix4f transform;
00206   computeTransformation (src, tgt, transform);
00207 
00208   std::cerr << transform << std::endl;
00209   // Transform the data and write it to disk
00210   PointCloud<PointXYZ> output;
00211   transformPointCloud (*src, output, transform);
00212   savePCDFileBinary ("source_transformed.pcd", output);
00213 }
00214 /* ]--- */


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