batchTesterHistogramFPFH.cc
Go to the documentation of this file.
00001 #include <NDTMap.hh>
00002 #include <NDTHistogram.hh>
00003 #include <OctTree.hh>
00004 #include <AdaptiveOctTree.hh>
00005 #include <PointCloudUtils.hh>
00006 
00007 #include "pcl/point_cloud.h"
00008 #include "pcl/io/pcd_io.h"
00009 #include "pcl/features/feature.h"
00010 #include "pcl/kdtree/kdtree_flann.h"
00011 #include "pcl/filters/passthrough.h"
00012 #include "pcl/filters/voxel_grid.h"
00013 #include "pcl/features/normal_3d.h"
00014 #include "pcl/features/fpfh.h"
00015 #include "pcl/registration/ia_ransac.h"
00016 #include <cstdio>
00017 
00018 #include <LazzyGrid.hh>
00019 #include <fstream>
00020 
00021 using namespace std;
00022 
00023 //CLASSES FOR FPFH MACHING
00024 
00025 class FeatureCloud
00026 {
00027 public:
00028     // A bit of shorthand
00029     typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00030     typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
00031     typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
00032     typedef pcl::KdTreeFLANN<pcl::PointXYZ> SearchMethod;
00033 
00034     FeatureCloud () :
00035         search_method_xyz_ (new SearchMethod),
00036         normal_radius_ (0.05),
00037         feature_radius_ (0.05)
00038     {
00039     }
00040 
00041     ~FeatureCloud () {}
00042 
00043     // Process the given cloud
00044     void
00045     setInputCloud (PointCloud::Ptr xyz)
00046     {
00047         xyz_ = xyz;
00048         processInput ();
00049     }
00050 
00051     // Get a pointer to the cloud 3D points
00052     PointCloud::Ptr
00053     getPointCloud () const
00054     {
00055         return (xyz_);
00056     }
00057 
00058     // Get a pointer to the cloud of 3D surface normals
00059     SurfaceNormals::Ptr
00060     getSurfaceNormals () const
00061     {
00062         return (normals_);
00063     }
00064 
00065     // Get a pointer to the cloud of feature descriptors
00066     LocalFeatures::Ptr
00067     getLocalFeatures () const
00068     {
00069         return (features_);
00070     }
00071 
00072 protected:
00073     // Compute the surface normals and local features
00074     void
00075     processInput ()
00076     {
00077         computeSurfaceNormals ();
00078         computeLocalFeatures ();
00079     }
00080 
00081     // Compute the surface normals
00082     void
00083     computeSurfaceNormals ()
00084     {
00085         normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
00086 
00087         pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
00088         norm_est.setInputCloud (xyz_);
00089         norm_est.setSearchMethod (search_method_xyz_);
00090         norm_est.setRadiusSearch (normal_radius_);
00091         norm_est.compute (*normals_);
00092     }
00093 
00094     // Compute the local feature descriptors
00095     void
00096     computeLocalFeatures ()
00097     {
00098         features_ = LocalFeatures::Ptr (new LocalFeatures);
00099 
00100         pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
00101         fpfh_est.setInputCloud (xyz_);
00102         fpfh_est.setInputNormals (normals_);
00103         fpfh_est.setSearchMethod (search_method_xyz_);
00104         fpfh_est.setRadiusSearch (feature_radius_);
00105         fpfh_est.compute (*features_);
00106     }
00107 
00108 private:
00109     // Point cloud data
00110     PointCloud::Ptr xyz_;
00111     SurfaceNormals::Ptr normals_;
00112     LocalFeatures::Ptr features_;
00113     SearchMethod::Ptr search_method_xyz_;
00114 
00115     // Parameters
00116     float normal_radius_;
00117     float feature_radius_;
00118 };
00119 
00120 class TemplateRegistration
00121 {
00122 public:
00123 
00124     // A struct for storing alignment results
00125 
00126     typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Result;
00127 
00128     TemplateRegistration () :
00129         min_sample_distance_ (0.05),
00130         max_correspondence_distance_ (0.01*0.01),
00131         nr_iterations_ (500)
00132     {
00133         // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
00134         sac_ia_.setMinSampleDistance (min_sample_distance_);
00135         sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
00136         sac_ia_.setMaximumIterations (nr_iterations_);
00137         sac_ia_.setNumberOfSamples (10);
00138     }
00139 
00140     ~TemplateRegistration () {}
00141 
00142     // Set the given cloud as the target to which the templates will be aligned
00143     void
00144     setTargetCloud (FeatureCloud &target_cloud)
00145     {
00146         target_ = target_cloud;
00147         sac_ia_.setInputTarget (target_cloud.getPointCloud ());
00148         sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
00149     }
00150 
00151     // Align the moving cloud to the target specified by setTargetCloud ()
00152     void
00153     align (FeatureCloud &moving_cloud, TemplateRegistration::Result &result)
00154     {
00155         sac_ia_.setInputCloud (moving_cloud.getPointCloud ());
00156         sac_ia_.setSourceFeatures (moving_cloud.getLocalFeatures ());
00157 
00158         pcl::PointCloud<pcl::PointXYZ> registration_output;
00159         sac_ia_.align (registration_output);
00160 
00161         result = sac_ia_.getFinalTransformation ().cast<double>();
00162     }
00163 
00164 private:
00165     // A list of template clouds and the target to which they will be aligned
00166     FeatureCloud target_;
00167 
00168     // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
00169     pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
00170     float min_sample_distance_;
00171     float max_correspondence_distance_;
00172     float nr_iterations_;
00173 };
00174 
00175 
00176 // Align two point clouds based on the features
00177 bool matchFPFH(pcl::PointCloud<pcl::PointXYZ> &fixed,  pcl::PointCloud<pcl::PointXYZ> &moving,
00178                Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout)
00179 {
00180 
00181     Tout.setIdentity();
00182 
00183     // Load the target cloud PCD file
00184     pcl::PointCloud<pcl::PointXYZ>::Ptr cloudM (new pcl::PointCloud<pcl::PointXYZ>);
00185     pcl::PointCloud<pcl::PointXYZ>::Ptr cloudF (new pcl::PointCloud<pcl::PointXYZ>);
00186     pcl::PointCloud<pcl::PointXYZ> Final, Final0, Final1;
00187 
00188     *cloudM = moving;
00189     *cloudF = fixed;
00190 
00191     pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
00192     gr1.setLeafSize(0.05,0.05,0.05);
00193     gr2.setLeafSize(0.05,0.05,0.05);
00194 
00195     gr1.setInputCloud(cloudM);
00196     gr2.setInputCloud(cloudF);
00197 
00198     gr1.filter(*cloudM);
00199     gr2.filter(*cloudF);
00200 
00201     cloudM->height = 1;
00202     cloudM->width = cloudM->points.size();
00203     cloudF->height = 1;
00204     cloudF->width = cloudF->points.size();
00205     cloudM->is_dense = false;
00206     cloudF->is_dense = false;
00207 
00208     // Assign to the target FeatureCloud
00209     FeatureCloud target_cloud, moving_cloud;
00210     target_cloud.setInputCloud (cloudF);
00211     moving_cloud.setInputCloud (cloudM);
00212 
00213     TemplateRegistration templateReg;
00214     templateReg.setTargetCloud (target_cloud);
00215 
00216     // Find the best template alignment
00217     templateReg.align(moving_cloud,Tout);
00218 
00219     return true;
00220 }
00221 
00222 int
00223 main (int argc, char** argv)
00224 {
00225 
00226     if(argc!=4)
00227     {
00228         cout<<"usage: "<<argv[0]<<" numberOfUniquePositions pathTotestDirectory outputFileName\n Data has to be in the \
00229             proper format!!!\n Assumptions: scans are 15 degrees apart, in groups of 10\n";
00230         return(-1);
00231     }
00232     int nPos = atoi(argv[1]);
00233     std::string prefix = argv[2];
00234     lslgeneric::OctTree tr;
00235     //lslgeneric::LazzyGrid tr(0.5);
00236     lslgeneric::OctTree::BIG_CELL_SIZE = 0.5;
00237     lslgeneric::OctTree::SMALL_CELL_SIZE = 0.2;
00238     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T_HIST, T_FPFH, Tinverse;
00239     Eigen::Quaternion<double> id, errorQ;
00240     id.setIdentity();
00241     double msec_HIST=0, msec_FPFH=0;
00242     struct timeval tv_start, tv_end;
00243 
00244     Tinverse = Eigen::AngleAxis<double>(-30*M_PI/180,Eigen::Vector3d::UnitZ());
00245     double angle_threshold = 5*M_PI/180;
00246     int N_TRUE_HIST = 0, N_TRUE_FPFH = 0;
00247     int N_FALSE_HIST= 0, N_FALSE_FPFH= 0;
00248     int N_PASS1=0, N_PASS2 = 0;
00249 
00250     std::vector<double> angleErrorsHIST, angleErrorsFPFH;
00251     std::vector<double> timesHIST, timesFPFH;
00252     for(int i=0; i<nPos; i++)
00253     {
00254         pcl::PointCloud<pcl::PointXYZ> cloud[10], cloud2, cloud3;
00255         for(int j=0; j<10; j++)
00256         {
00257             char fname[500];
00258             snprintf(fname,499,"%s%03d.wrl",prefix.c_str(),10*i+j);
00259             cloud[j] = lslgeneric::readVRML(fname);
00260         }
00261 
00262         for(int j=2; j<10; j++)
00263         {
00264 
00265             //HISTOGRAM
00266             //start timing
00267             gettimeofday(&tv_start,NULL);
00268             //compare clouds of j and j-2
00269             //lslgeneric::NDTMap nd(new lslgeneric::LazzyGrid(0.5));
00270             lslgeneric::NDTMap nd1(&tr);
00271             nd1.loadPointCloud(cloud[j-2]);
00272             //lslgeneric::NDTMap nd2(new lslgeneric::LazzyGrid(0.5));
00273             lslgeneric::NDTMap nd2(&tr);
00274             nd2.loadPointCloud(cloud[j]);
00275 
00276             nd1.computeNDTCells();
00277             nd2.computeNDTCells();
00278 
00279             lslgeneric::NDTHistogram nh1(nd1);
00280             lslgeneric::NDTHistogram nh2(nd2);
00281 
00282             //find the distance from nh1 to nh2
00283             nh2.bestFitToHistogram(nh1,T_HIST);
00284             //stop timing1
00285             gettimeofday(&tv_end,NULL);
00286             msec_HIST += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00287 
00288             T_HIST = T_HIST*Tinverse;
00289             errorQ = T_HIST.rotation();
00290             double angle = acos(id.dot(errorQ))/2;
00291             angleErrorsHIST.push_back(angle);
00292             timesHIST.push_back((tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.);
00293 
00294             std::cout<<"h: "<<T_HIST.rotation().eulerAngles(0,1,2).transpose()<<" --> "<<angle<<endl;
00295             if(fabsf(angle) < angle_threshold)
00296             {
00297                 cout<< "PASS\n";
00298                 N_TRUE_HIST++;
00299             }
00300             else
00301             {
00302                 N_FALSE_HIST++;
00303                 //TRY SECOND BEST MATCH
00304                 nh2.getTransform(1,T_HIST);
00305                 T_HIST = T_HIST*Tinverse;
00306                 errorQ = T_HIST.rotation();
00307                 double angle = acos(id.dot(errorQ))/2;
00308 
00309                 std::cout<<"h: "<<T_HIST.rotation().eulerAngles(0,1,2).transpose()<<" --> "<<angle<<endl;
00310                 if(fabsf(angle) < angle_threshold)
00311                 {
00312                     cout<< ">>>>>>>>>>>>>> PASS 2\n";
00313                     N_PASS1++;
00314                 }
00315                 else
00316                 {
00317                     nh2.getTransform(2,T_HIST);
00318                     T_HIST = T_HIST*Tinverse;
00319                     errorQ = T_HIST.rotation();
00320                     double angle = acos(id.dot(errorQ))/2;
00321 
00322                     std::cout<<"h: "<<T_HIST.rotation().eulerAngles(0,1,2).transpose()<<" --> "<<angle<<endl;
00323                     if(fabsf(angle) < angle_threshold)
00324                     {
00325                         cout<< ">>>>>>>>>>>>>> PASS 3\n";
00326                         N_PASS2++;
00327                     }
00328                     else
00329                     {
00330                         cout<< "FAIL "<<angle*180/M_PI<<endl;
00331                     }
00332                 }
00333             }
00334 
00335             //double sim = nh2.getSimilarity(nh1);
00336             //cout<<"scan similarity "<<sim<<endl;
00337 
00338             //FPFH fixed = nd1 = cloud[j-2], moving = nd2
00339             //start timing
00340             gettimeofday(&tv_start,NULL);
00341             matchFPFH(cloud[j-2],cloud[j],T_FPFH);
00342             //stop timing1
00343             gettimeofday(&tv_end,NULL);
00344             msec_FPFH += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00345 
00346             T_FPFH = T_FPFH*Tinverse;
00347             errorQ = T_FPFH.rotation();
00348             angle = acos(id.dot(errorQ))/2;
00349             angleErrorsFPFH.push_back(angle);
00350             timesFPFH.push_back((tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.);
00351 
00352             std::cout<<"f: "<<T_FPFH.rotation().eulerAngles(0,1,2).transpose()<<" --> "<<angle<<endl;
00353             if(fabsf(angle) < angle_threshold)
00354             {
00355                 cout<< "PASS\n";
00356                 N_TRUE_FPFH++;
00357             }
00358             else
00359             {
00360                 cout<< "FAIL "<<angle*180/M_PI<<endl;
00361                 N_FALSE_FPFH++;
00362             }
00363             /*
00364             */
00365             /*
00366                     //OUTPUT:
00367                     char fname[500];
00368                     snprintf(fname,499,"%03d.wrl",10*i+j);
00369                     cloud2 = lslgeneric::transformPointCloud(T_HIST,cloud[j-2]);
00370                     //cloud3 = lslgeneric::transformPointCloud(T_FPFH,cloud[j-2]);
00371 
00372                     FILE *f = fopen(fname,"w");
00373                     fprintf(f,"#VRML V2.0 utf8\n");
00374                     //green = target
00375                     lslgeneric::writeToVRML(f,cloud[j-2],Eigen::Vector3d(0,1,0));
00376                     //red = init
00377                     lslgeneric::writeToVRML(f,cloud[j],Eigen::Vector3d(1,0,0));
00378                     //white = final hist
00379                     lslgeneric::writeToVRML(f,cloud2,Eigen::Vector3d(1,1,1));
00380                     //blue = final fpfh
00381                     //lslgeneric::writeToVRML(f,cloud3,Eigen::Vector3d(0,0,1));
00382                     fclose(f);
00383             */
00384         }
00385     }
00386 
00387     cout<<"=================================\n HISTOGRAM: \n SUCC: "
00388         <<N_TRUE_HIST<<" FAIL: "<<N_FALSE_HIST<<" P1 "<<N_PASS1<<" P2 "<<N_PASS2<<" avg time "<<msec_HIST/(nPos*10)<<" msec\n FPFH: \n SUCC: "
00389         <<N_TRUE_FPFH<<" FAIL: "<<N_FALSE_FPFH<<" avg time "<<msec_FPFH/(nPos*10)<<" msec\n";
00390 
00391     ofstream logger(argv[3]);
00392 
00393     logger<<"Eh = [";
00394     for(int i=0; i<angleErrorsHIST.size(); i++)
00395     {
00396         logger<<angleErrorsHIST[i]<<" ";
00397     }
00398     logger<<"];\n";
00399 
00400     logger<<"Ef = [";
00401     for(int i=0; i<angleErrorsFPFH.size(); i++)
00402     {
00403         logger<<angleErrorsFPFH[i]<<" ";
00404     }
00405     logger<<"];\n";
00406 
00407     logger<<"Th = [";
00408     for(int i=0; i<timesHIST.size(); i++)
00409     {
00410         logger<<timesHIST[i]<<" ";
00411     }
00412     logger<<"];\n";
00413 
00414     logger<<"Tf = [";
00415     for(int i=0; i<timesFPFH.size(); i++)
00416     {
00417         logger<<timesFPFH[i]<<" ";
00418     }
00419     logger<<"];\n";
00420 
00421     logger<<"succh = "<<N_TRUE_HIST<<"; failh = "<<N_FALSE_HIST
00422           <<"; succf= "<<N_TRUE_FPFH<<"; failf = "<<N_FALSE_FPFH<<";\n"<<" P1 = "<<N_PASS1<<" P2= "<<N_PASS2<<"\n;";
00423 
00424     return (0);
00425 }
00426 
00427 
00428 


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:31:57