test_ndt_mapper.cc
Go to the documentation of this file.
00001 #include<NDTMapBuilder.hh>
00002 #include <OctTree.hh>
00003 #include <NDTMatcherF2F.hh>
00004 #include <PointCloudUtils.hh>
00005 
00006 using namespace std;
00007 using namespace lslgeneric;
00008 
00009 int main(int argc, char**argv)
00010 {
00011 
00012     if(argc != 7)
00013     {
00014         cout<<"Usage: "<<argv[0]<<" N_CLOUDS clouds_prefix (bKinect|bLaser) (doHistogram=0|1) (bD2D|bP2D|bICP) offset\n \
00015             \t bKinect -> use settings more suitable for a kinect\n \
00016             \t bLaser -> use settings more suitable for a kinect\n \
00017             \t bD2D . p2D ICP -> choice of registration algorithm\n";
00018         return -1;
00019     }
00020 
00021     bool bKinect = (strncmp(argv[3],"bKinect",7) == 0);
00022     bool doHistogram = (atoi(argv[4]) == 1);
00023     bool bICP = (strncmp(argv[5],"bICP",4) == 0);
00024     bool bP2D = (strncmp(argv[5],"bP2D",4) == 0);
00025     bool bD2D = ((strncmp(argv[5],"bD2D",4) == 0) || !(bICP || bP2D));
00026     int offset = atoi(argv[6]);
00027 
00028     lslgeneric::OctTree histogramPrototype;
00029     std::vector<double> resolutions;
00030 
00031     double __res1[] = {0.5, 1, 2, 4};
00032     double __res2[] = {0.1,0.2,1,2,4};
00033     if(!bKinect)
00034     {
00035         resolutions = std::vector<double>(__res1, __res1+sizeof(__res1)/sizeof(double));
00036         lslgeneric::OctTree::BIG_CELL_SIZE = 2;
00037         lslgeneric::OctTree::SMALL_CELL_SIZE = 0.2;
00038 
00039     }
00040     else
00041     {
00042         resolutions = std::vector<double>(__res2, __res2+sizeof(__res2)/sizeof(double));
00043         lslgeneric::OctTree::BIG_CELL_SIZE = 0.5;
00044         lslgeneric::OctTree::SMALL_CELL_SIZE = 0.1;
00045 
00046     }
00047     lslgeneric::OctTree::parametersSet = true;
00048     lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);
00049     lslgeneric::NDTMatcher matcherP2F(resolutions);
00050 
00051     NDTMapBuilder mapper(doHistogram);
00052     if(bD2D)
00053     {
00054         mapper.setMatcherF2F(&matcherF2F);
00055         cout<<"setting to D2D matcher\n";
00056     }
00057     else if(bP2D)
00058     {
00059         mapper.setMatcherP2F(&matcherP2F);
00060         cout<<"setting to P2D matcher\n";
00061     }
00062     else
00063     {
00064         mapper.setICP();
00065         cout<<"setting to ICP matcher\n";
00066     }
00067 
00068     mapper.tr = histogramPrototype;
00069 
00070     int N_CLOUDS = atoi(argv[1]);
00071     char fname[600];
00072 
00073     double MAX_DIST = 26;
00074 
00075     if(bKinect) MAX_DIST = 5;
00076 
00077     for (int i=offset; i<N_CLOUDS; i++)
00078     {
00079         snprintf(fname,600,"%s%03d.wrl",argv[2],i);
00080         cout<<fname<<endl;
00081         pcl::PointCloud<pcl::PointXYZ> cl = lslgeneric::readVRML(fname);
00082         pcl::PointCloud<pcl::PointXYZ> filtered;
00083         for(int q=0; q<cl.points.size(); q++)
00084         {
00085             double dist = sqrt(pow(cl.points[q].x,2)+pow(cl.points[q].y,2)+pow(cl.points[q].z,2));
00086             if(dist<MAX_DIST)
00087             {
00088                 if(!bKinect)
00089                 {
00090                     filtered.points.push_back(cl.points[q]);
00091                 }
00092                 else
00093                 {
00094                     pcl::PointXYZ pNew;
00095                     pNew.x = cl.points[q].z;
00096                     pNew.y = cl.points[q].x;
00097                     pNew.z = -cl.points[q].y;
00098                     filtered.points.push_back(pNew);
00099                 }
00100 
00101             }
00102         }
00103 
00104         cout<<"adding cloud number "<<i<<endl;
00105         mapper.addScan(filtered);
00106     }
00107 
00108     snprintf(fname,600,"%s.g2o",argv[2]);
00109     mapper.saveG2OlogFile(fname);
00110 
00111     snprintf(fname,600,"%s.dat",argv[2]);
00112     mapper.saveDatlogFile(fname);
00113 
00114     snprintf(fname,600,"%s_COMPLETE.wrl",argv[2]);
00115     mapper.theMotherOfAllPointClouds(fname);
00116 
00117     return 0;
00118 }


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