feature_registration.cc
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 #include "pcl/registration/icp.h"
00015 #include <pcl/surface/mls.h>
00016 
00017 #include <NDTMatcherF2F.hh>
00018 #include <PointCloudUtils.hh>
00019 
00020 class FeatureCloud
00021 {
00022 public:
00023     // A bit of shorthand
00024     typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00025     typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
00026     typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
00027     //typedef pcl::KdTreeFLANN<pcl::PointXYZ> SearchMethod;
00028 
00029 
00030     FeatureCloud () :
00031         //search_method_xyz_ (new SearchMethod),
00032         normal_radius_ (0.05),
00033         feature_radius_ (0.05)
00034     {
00035     }
00036 
00037     ~FeatureCloud () {}
00038 
00039     // Process the given cloud
00040     void
00041     setInputCloud (PointCloud::Ptr xyz)
00042     {
00043         xyz_ = xyz;
00044         processInput ();
00045     }
00046 
00047     // Load and process the cloud in the given PCD file
00048     void
00049     loadInputCloud (const std::string &pcd_file)
00050     {
00051         xyz_ = PointCloud::Ptr (new PointCloud);
00052         pcl::io::loadPCDFile (pcd_file, *xyz_);
00053         processInput ();
00054     }
00055 
00056     // Get a pointer to the cloud 3D points
00057     PointCloud::Ptr
00058     getPointCloud () const
00059     {
00060         return (xyz_);
00061     }
00062 
00063     // Get a pointer to the cloud of 3D surface normals
00064     SurfaceNormals::Ptr
00065     getSurfaceNormals () const
00066     {
00067         return (normals_);
00068     }
00069 
00070     // Get a pointer to the cloud of feature descriptors
00071     LocalFeatures::Ptr
00072     getLocalFeatures () const
00073     {
00074         return (features_);
00075     }
00076 
00077 protected:
00078     // Compute the surface normals and local features
00079     void
00080     processInput ()
00081     {
00082         computeSurfaceNormals ();
00083         computeLocalFeatures ();
00084     }
00085 
00086     // Compute the surface normals
00087     void
00088     computeSurfaceNormals ()
00089     {
00090         normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
00091 
00092         pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
00093         norm_est.setInputCloud (xyz_);
00094         norm_est.setSearchMethod (search_method_xyz_);
00095         norm_est.setRadiusSearch (normal_radius_);
00096         norm_est.compute (*normals_);
00097     }
00098 
00099     // Compute the local feature descriptors
00100     void
00101     computeLocalFeatures ()
00102     {
00103         features_ = LocalFeatures::Ptr (new LocalFeatures);
00104 
00105         pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
00106         fpfh_est.setInputCloud (xyz_);
00107         fpfh_est.setInputNormals (normals_);
00108         fpfh_est.setSearchMethod (search_method_xyz_);
00109         fpfh_est.setRadiusSearch (feature_radius_);
00110         fpfh_est.compute (*features_);
00111     }
00112 
00113 private:
00114     // Point cloud data
00115     PointCloud::Ptr xyz_;
00116     SurfaceNormals::Ptr normals_;
00117     LocalFeatures::Ptr features_;
00118     //SearchMethod::Ptr search_method_xyz_;
00119     pcl::Feature<pcl::PointXYZ, pcl::Normal>::KdTreePtr search_method_xyz_;
00120 
00121     // Parameters
00122     float normal_radius_;
00123     float feature_radius_;
00124 };
00125 
00126 class TemplateRegistration
00127 {
00128 public:
00129 
00130     // A struct for storing alignment results
00131 
00132     typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Result;
00133 
00134     TemplateRegistration () :
00135         min_sample_distance_ (0.05),
00136         max_correspondence_distance_ (0.01*0.01),
00137         nr_iterations_ (500)
00138     {
00139         // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
00140         sac_ia_.setMinSampleDistance (min_sample_distance_);
00141         sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
00142         sac_ia_.setMaximumIterations (nr_iterations_);
00143         sac_ia_.setNumberOfSamples (10);
00144     }
00145 
00146     ~TemplateRegistration () {}
00147 
00148     // Set the given cloud as the target to which the templates will be aligned
00149     void
00150     setTargetCloud (FeatureCloud &target_cloud)
00151     {
00152         target_ = target_cloud;
00153         sac_ia_.setInputTarget (target_cloud.getPointCloud ());
00154         sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
00155     }
00156 
00157     // Align the moving cloud to the target specified by setTargetCloud ()
00158     void
00159     align (FeatureCloud &moving_cloud, TemplateRegistration::Result &result)
00160     {
00161         sac_ia_.setInputCloud (moving_cloud.getPointCloud ());
00162         sac_ia_.setSourceFeatures (moving_cloud.getLocalFeatures ());
00163 
00164         pcl::PointCloud<pcl::PointXYZ> registration_output;
00165         sac_ia_.align (registration_output);
00166 
00167         result = sac_ia_.getFinalTransformation ().cast<double>();
00168     }
00169 
00170 private:
00171     // A list of template clouds and the target to which they will be aligned
00172     FeatureCloud target_;
00173 
00174     // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
00175     pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
00176     float min_sample_distance_;
00177     float max_correspondence_distance_;
00178     float nr_iterations_;
00179 };
00180 
00181 bool matchICP(pcl::PointCloud<pcl::PointXYZ> &fixed,  pcl::PointCloud<pcl::PointXYZ> &moving,
00182               Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout)
00183 {
00184 
00185     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
00186     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
00187     pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(fixed) );
00188     pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(moving) );
00189 
00190     pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
00191     gr1.setLeafSize(0.1,0.1,0.1);
00192     gr2.setLeafSize(0.1,0.1,0.1);
00193 
00194     gr1.setInputCloud(m);
00195     gr2.setInputCloud(f);
00196 
00197     cloud_in->height = 1;
00198     cloud_in->width = cloud_in->points.size();
00199     cloud_out->height = 1;
00200     cloud_out->width = cloud_out->points.size();
00201     cloud_in->is_dense = false;
00202     cloud_out->is_dense = false;
00203 
00204     gr1.filter(*cloud_in);
00205     gr2.filter(*cloud_out);
00206     //*cloud_in = moving;
00207     //*cloud_out= fixed;
00208 
00209 
00210     pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
00211 
00212     icp.setMaximumIterations(10000);
00213     std::cout<<"max itr are "<<icp.getMaximumIterations()<<std::endl;
00214     icp.setInputCloud(cloud_in);
00215     icp.setInputTarget(cloud_out);
00216 
00217     icp.setRANSACOutlierRejectionThreshold (2);
00218     icp.setMaxCorrespondenceDistance(10);
00219     icp.setTransformationEpsilon(0.00001);
00220 //    cout<<"ransac outlier thersh   : "<<icp.getRANSACOutlierRejectionThreshold ()<<endl;
00221 //    cout<<"correspondance max dist : "<<icp.getMaxCorrespondenceDistance() << endl;
00222 //    cout<<"epsilon : "<<icp.getTransformationEpsilon() << endl;
00223     pcl::PointCloud<pcl::PointXYZ> Final;
00224     icp.align(Final);
00225 
00226 
00227 //    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
00228 //      icp.getFitnessScore() << std::endl;
00229 //    std::cout << icp.getFinalTransformation() << std::endl;
00230 
00231     //Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> tTemp;
00232     Tout = (icp.getFinalTransformation()).cast<double>();
00233 
00234     /*    char fname[50];
00235         snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
00236         FILE *fout = fopen(fname,"w");
00237         fprintf(fout,"#VRML V2.0 utf8\n");
00238         lslgeneric::writeToVRML(fout,*cloud_out,Eigen::Vector3d(0,1,0));
00239         lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(1,0,0));
00240         lslgeneric::writeToVRML(fout,*cloud_in,Eigen::Vector3d(1,1,1));
00241         fclose(fout);
00242     */
00243     return icp.hasConverged();
00244 
00245 }
00246 
00247 // Align two point clouds based on the features
00248 int
00249 main (int argc, char **argv)
00250 {
00251     if (argc < 3)
00252     {
00253         printf ("No targets given!\n");
00254         return (-1);
00255     }
00256 
00257     struct timeval tv_start, tv_end1, tv_end2;
00258     TemplateRegistration::Result ToutFPFH, ToutICP, ToutNDT, Tout;
00259 
00260     // Load the target cloud PCD file
00261     pcl::PointCloud<pcl::PointXYZ>::Ptr cloudM (new pcl::PointCloud<pcl::PointXYZ>);
00262     pcl::PointCloud<pcl::PointXYZ>::Ptr cloudF (new pcl::PointCloud<pcl::PointXYZ>);
00263     pcl::PointCloud<pcl::PointXYZ> Final, Final0, Final1;
00264 
00265     *cloudM = lslgeneric::readVRML(argv[1]);
00266     *cloudF = lslgeneric::readVRML(argv[2]);
00267 
00268     double __res[] = {0.2, 0.4, 1, 2};
00269     std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00270     lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);
00271     bool ret = matcherF2F.match(*cloudF,*cloudM,ToutNDT);
00272     Final1 = lslgeneric::transformPointCloud(ToutNDT,*cloudM);
00273 
00274     /* char f[50];
00275      snprintf(f,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
00276      FILE *fo = fopen(f,"w");
00277      fprintf(fo,"#VRML V2.0 utf8\n");
00278      lslgeneric::writeToVRML(fo,*cloudM,Eigen::Vector3d(1,0,0));
00279      lslgeneric::writeToVRML(fo,Final1,Eigen::Vector3d(0,1,1));
00280      lslgeneric::writeToVRML(fo,*cloudF,Eigen::Vector3d(1,1,1));
00281      fclose(fo);
00282      return (0);
00283      */
00284     //start timing
00285     gettimeofday(&tv_start,NULL);
00286 
00287     pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
00288     gr1.setLeafSize(0.05,0.05,0.05);
00289     gr2.setLeafSize(0.05,0.05,0.05);
00290 
00291     gr1.setInputCloud(cloudM);
00292     gr2.setInputCloud(cloudF);
00293 
00294     gr1.filter(*cloudM);
00295     gr2.filter(*cloudF);
00296 
00297     cloudM->height = 1;
00298     cloudM->width = cloudM->points.size();
00299     cloudF->height = 1;
00300     cloudF->width = cloudF->points.size();
00301     cloudM->is_dense = false;
00302     cloudF->is_dense = false;
00303 
00304     // Assign to the target FeatureCloud
00305     FeatureCloud target_cloud, moving_cloud;
00306     target_cloud.setInputCloud (cloudF);
00307     moving_cloud.setInputCloud (cloudM);
00308 
00309     TemplateRegistration templateReg;
00310     templateReg.setTargetCloud (target_cloud);
00311 
00312     // Find the best template alignment
00313     templateReg.align(moving_cloud,ToutFPFH);
00314     //stop timing1
00315     gettimeofday(&tv_end1,NULL);
00316 
00317     std::cout<<"ToutFPFH: "<<ToutFPFH.translation().transpose()<<"\n"<<ToutFPFH.rotation()<<std::endl;
00318     Final0 = lslgeneric::transformPointCloud(ToutFPFH,*cloudM);
00319 
00320     bool converged = matchICP(*cloudF,Final0,ToutICP);
00321     Final = lslgeneric::transformPointCloud(ToutICP,Final0);
00322     //stop timing2
00323     gettimeofday(&tv_end2,NULL);
00324 
00325 
00326     Tout = ToutFPFH*(ToutNDT.inverse());
00327     std::cout<<"FPFH\n";
00328     std::cout<<"E translation "<<Tout.translation().transpose()
00329              <<" (norm) "<<Tout.translation().norm()<<std::endl;
00330     std::cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
00331              <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00332     std::cout<<" TIME: "<<
00333              (tv_end1.tv_sec-tv_start.tv_sec)*1000.+(tv_end1.tv_usec-tv_start.tv_usec)/1000.<<std::endl;
00334 
00335     Tout = ToutICP*ToutFPFH*(ToutNDT.inverse());
00336     std::cout<<"FPFH+ICP\n";
00337     std::cout<<"E translation "<<Tout.translation().transpose()
00338              <<" (norm) "<<Tout.translation().norm()<<std::endl;
00339     std::cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
00340              <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00341     std::cout<<" TIME: "<<
00342              (tv_end2.tv_sec-tv_start.tv_sec)*1000.+(tv_end2.tv_usec-tv_start.tv_usec)/1000.<<std::endl;
00343 
00344     char fname[50];
00345     snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
00346     FILE *fout = fopen(fname,"w");
00347     fprintf(fout,"#VRML V2.0 utf8\n");
00348     lslgeneric::writeToVRML(fout,*cloudM,Eigen::Vector3d(1,0,0));
00349     lslgeneric::writeToVRML(fout,Final0,Eigen::Vector3d(0,0,1));
00350     lslgeneric::writeToVRML(fout,Final1,Eigen::Vector3d(0,1,1));
00351     lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(0,1,0));
00352     lslgeneric::writeToVRML(fout,*cloudF,Eigen::Vector3d(1,1,1));
00353     fclose(fout);
00354 
00355 
00356 
00357     return (0);
00358 }


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Wed Aug 26 2015 15:24:52