ndt_histogram_registration.cc
Go to the documentation of this file.
00001 #include <NDTMatcherF2F.hh>
00002 #include <NDTMap.hh>
00003 #include <NDTHistogram.hh>
00004 #include <OctTree.hh>
00005 #include <AdaptiveOctTree.hh>
00006 #include "ros/ros.h"
00007 #include "pcl/point_cloud.h"
00008 #include "sensor_msgs/PointCloud2.h"
00009 #include "pcl/io/pcd_io.h"
00010 #include "pcl/features/feature.h"
00011 #include "pcl/registration/icp.h"
00012 #include "pcl/filters/voxel_grid.h"
00013 #include <LazzyGrid.hh>
00014 
00015 #include <cstdio>
00016 #include <Eigen/Eigen>
00017 #include <PointCloudUtils.hh>
00018 #include <fstream>
00019 
00020 using namespace std;
00021 
00022 int
00023 main (int argc, char** argv)
00024 {
00025 
00026     std::ofstream logger ("/home/tsv/ndt_tmp/results_histogram.txt");
00027     //cout.precision(15);
00028 
00029     pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3, cloud4, cloud5, cloud6, cloud7 ;
00030     double __res[] = {0.5, 1, 2, 4};
00031     std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00032     lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);
00033 
00034     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tin, Tout, Tndt;
00035 
00036     struct timeval tv_start, tv_end;
00037 
00038     double bestscore = INT_MAX;
00039 
00040     Tout.setIdentity();
00041     bool succ = false;
00042     if(argc == 3)
00043     {
00044         //we do a single scan to scan registration
00045 
00046         cloud1 = lslgeneric::readVRML(argv[1]);
00047         cloud2 = lslgeneric::readVRML(argv[2]);
00048 
00049         // lslgeneric::AdaptiveOctTree::MIN_CELL_SIZE = 0.01;
00050         lslgeneric::OctTree tr;
00051         //lslgeneric::LazzyGrid tr(0.5);
00052         lslgeneric::OctTree::BIG_CELL_SIZE = 1;
00053         lslgeneric::OctTree::SMALL_CELL_SIZE = 0.2;
00054         double finalscore;
00055 
00056         gettimeofday(&tv_start,NULL);
00057 
00058         lslgeneric::NDTMap fixed(&tr);
00059         fixed.loadPointCloud(cloud1);
00060         lslgeneric::NDTMap moving(&tr);
00061         moving.loadPointCloud(cloud2);
00062         lslgeneric::NDTMap moved(&tr);
00063 
00064 
00065         fixed.computeNDTCells();
00066         moving.computeNDTCells();
00067 
00068         lslgeneric::NDTHistogram fixedH(fixed);
00069         lslgeneric::NDTHistogram movingH(moving);
00070 
00071         //cout<<"1 =========== \n";
00072         //fixedH.printHistogram(true);
00073         //cout<<"2 =========== \n";
00074         //movingH.printHistogram(true);
00075 
00076         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;
00077 
00078         movingH.bestFitToHistogram(fixedH,T);
00079 
00080         //cout<<" ==================== \n Transform R "<<T.rotation()<<"\nt "<<T.translation().transpose()<<endl;
00081 
00082         for(int q=0; q<1; q++)
00083         {
00084             if(q!=3)
00085             {
00086                 movingH.getTransform(q,T);
00087             }
00088             else
00089             {
00090                 T.setIdentity();
00091             }
00092             cout<<"T init "<<T.translation().transpose()<<" r "<<T.rotation().eulerAngles(0,1,2).transpose()<<endl;
00093             Tndt.setIdentity();
00094             cloud3 = lslgeneric::transformPointCloud(T,cloud2);
00095             bool ret = matcherF2F.match(cloud1,cloud3,Tndt);
00096             finalscore = matcherF2F.finalscore;
00097             cout<<"final score at "<<q<<" is "<<finalscore<<endl;
00098             if(finalscore < bestscore)
00099             {
00100                 Tout = Tndt*T;
00101                 bestscore = finalscore;
00102                 cout<<"score = "<<bestscore<<"best is "<<q<<endl;
00103             }
00104             cout<<"T fin "<<Tout.translation().transpose()<<" r "<<Tout.rotation().eulerAngles(0,1,2).transpose()<<endl;
00105         }
00106 
00107         cloud4 = lslgeneric::transformPointCloud(Tout,cloud2);
00108         /*
00109                 //movingH.getTransform(1,T);
00110                 //moved.loadPointCloud(cloud3);
00111                 //moved.computeNDTCells();
00112                 //bool ret = matcherF2F.match(fixed,moved,Tout);
00113                 bool ret = matcherF2F.match(cloud1,cloud3,Tndt);
00114                 finalscore = matcherF2F.finalscore;
00115                 if(finalscore < bestscore) {
00116                     Tout = Tndt;
00117                     bestscore = finalscore;
00118                     cout<<"score = "<<bestscore<<"best is 1!\n";
00119                     cloud4 = lslgeneric::transformPointCloud(Tndt,cloud3);
00120                 }
00121 
00122                 //option2
00123                 Tndt.setIdentity();
00124                 movingH.getTransform(1,T);
00125                 cloud3 = lslgeneric::transformPointCloud(T,cloud2);
00126                 ret = matcherF2F.match(cloud1,cloud3,Tndt);
00127                 finalscore = matcherF2F.finalscore;
00128                 if(finalscore < bestscore) {
00129                     Tout = Tndt;
00130                     bestscore = finalscore;
00131                     cout<<"score = "<<bestscore<<"best is 2!\n";
00132                     cloud4 = lslgeneric::transformPointCloud(Tndt,cloud3);
00133                 }
00134                 //cloud5 = lslgeneric::transformPointCloud(Tndt,cloud3);
00135 
00136                 //option3
00137                 Tndt.setIdentity();
00138                 movingH.getTransform(2,T);
00139                 cloud3 = lslgeneric::transformPointCloud(T,cloud2);
00140                 ret = matcherF2F.match(cloud1,cloud3,Tndt);
00141                 finalscore = matcherF2F.finalscore;
00142                 if(finalscore < bestscore) {
00143                     Tout = Tndt;
00144                     bestscore = finalscore;
00145                     cout<<"score = "<<bestscore<<"best is 3!\n";
00146                     cloud4 = lslgeneric::transformPointCloud(Tndt,cloud3);
00147                 }
00148                 //cloud6 = lslgeneric::transformPointCloud(Tndt,cloud3);
00149 
00150                 //option4
00151                 Tndt.setIdentity();
00152                 ret = matcherF2F.match(cloud1,cloud2,Tndt);
00153                 finalscore = matcherF2F.finalscore;
00154                 if(finalscore < bestscore) {
00155                     Tout = Tndt;
00156                     bestscore = finalscore;
00157                     cout<<"score = "<<bestscore<<"best is 4!\n";
00158                     cloud4 = lslgeneric::transformPointCloud(Tndt,cloud2);
00159                 }
00160                 //cloud7 = lslgeneric::transformPointCloud(Tndt,cloud2);
00161         */
00162         gettimeofday(&tv_end,NULL);
00163 
00164         cout<<" TIME: "<<
00165             (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.<<endl;
00166 
00167         char fname[50];
00168         snprintf(fname,49,"/home/tsv/ndt_tmp/c_offset.wrl");
00169         FILE *fout = fopen(fname,"w");
00170         fprintf(fout,"#VRML V2.0 utf8\n");
00171         //green = target
00172         lslgeneric::writeToVRML(fout,cloud1,Eigen::Vector3d(0,1,0));
00173         //red = before histogram
00174         lslgeneric::writeToVRML(fout,cloud2,Eigen::Vector3d(1,0,0));
00175         //blue = after histogram
00176         //lslgeneric::writeToVRML(fout,cloud3,Eigen::Vector3d(0,0,1));
00177 
00178         {
00179             Eigen::Vector3d out = Tout.rotation().eulerAngles(0,1,2);
00180             cout<<"rot: "<<out.transpose()<<endl;
00181             cout<<"translation "<<Tout.translation().transpose()<<endl;
00182             logger<<"rot: "<<out.transpose()<<endl;
00183             logger<<"translation "<<Tout.translation()<<endl;
00184             //white = after registration
00185             lslgeneric::writeToVRML(fout,cloud4,Eigen::Vector3d(1,1,1));
00186             //lslgeneric::writeToVRML(fout,cloud5,Eigen::Vector3d(0.6,0.6,0.6));
00187             //lslgeneric::writeToVRML(fout,cloud6,Eigen::Vector3d(1,1,1));
00188             //lslgeneric::writeToVRML(fout,cloud7,Eigen::Vector3d(0,1,1));
00189         }
00190 
00191         fclose(fout);
00192 
00193         succ = true;
00194     }
00195     return 0;
00196 }
00197 
00198 
00199 


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:32:03