p2d_deathmatch.cc
Go to the documentation of this file.
00001 #include <ndt_registration/ndt_matcher_p2d.h>
00002 //#include <ndt_registration/ndt_matcher_d2d_2d.h>
00003 //#include <ndt_registration/ndt_matcher_d2d.h>
00004 #include <ndt_map/ndt_map.h>
00005 #include <ndt_map/pointcloud_utils.h>
00006 
00007 #include "pcl/point_cloud.h"
00008 #include "pcl/io/pcd_io.h"
00009 #include <cstdio>
00010 #include <Eigen/Eigen>
00011 #include <Eigen/Geometry>
00012 
00013 #include <iostream>
00014 #include <sstream>
00015 
00016 using namespace std;
00017 
00018 #define DODBG
00019 
00020 double getDoubleTime()
00021 {
00022     struct timeval time;
00023     gettimeofday(&time,NULL);
00024     return time.tv_sec + time.tv_usec * 1e-6;
00025 }
00026 
00027 
00028 int
00029 main (int argc, char** argv)
00030 {
00031     if(argc<22) {
00032         std::cout<<"usage "<<argv[0]<<" cloud_fixed.pcd cloud_offset.pcd T00 T01 T02 T03 T10 T11 T12 T13 T20 T21 T22 T23 T30 T31 T32 T33 max_resolution n_iterations subsample_size\n";
00033         return -1;
00034     }
00035 
00036     pcl::PointCloud<pcl::PointXYZ> cloud_fixed, cloud_offset, cloud_trans;
00037     if (pcl::io::loadPCDFile (argv[1], cloud_fixed) == -1)
00038     {
00039         cerr << "Was not able to open file \""<<argv[1]<<"\".\n";
00040         return 1;
00041     }
00042 
00043     if (pcl::io::loadPCDFile (argv[2], cloud_offset) == -1)
00044     {
00045         cerr << "Was not able to open file \""<<argv[2]<<"\".\n";
00046         return 1;
00047     }
00048     cloud_trans = cloud_offset;
00049 
00050     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tinit;
00051     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tout;
00052     Tout.setIdentity();
00053 
00054     Tinit.matrix()(0,0) = atof(argv[3]);
00055     Tinit.matrix()(0,1) = atof(argv[4]);
00056     Tinit.matrix()(0,2) = atof(argv[5]);
00057     Tinit.matrix()(0,3) = atof(argv[6]);
00058    
00059     Tinit.matrix()(1,0) = atof(argv[7]);
00060     Tinit.matrix()(1,1) = atof(argv[8]);
00061     Tinit.matrix()(1,2) = atof(argv[9]);
00062     Tinit.matrix()(1,3) = atof(argv[10]);
00063    
00064     Tinit.matrix()(2,0) = atof(argv[11]);
00065     Tinit.matrix()(2,1) = atof(argv[12]);
00066     Tinit.matrix()(2,2) = atof(argv[13]);
00067     Tinit.matrix()(2,3) = atof(argv[14]);
00068 
00069     Tinit.matrix()(3,0) = 0;
00070     Tinit.matrix()(3,1) = 0;
00071     Tinit.matrix()(3,2) = 0;
00072     Tinit.matrix()(3,3) = 1;
00073 #ifdef DODBG
00074     std::cerr<<"Tinit:\n"<<Tinit.matrix()<<std::endl;
00075 #endif
00076 
00077     double tnow, tend;
00078     lslgeneric::transformPointCloudInPlace(Tinit,cloud_offset);
00079 
00080     tnow = getDoubleTime();
00081     double res_max = atof(argv[19]);
00082     int itr_max = atoi(argv[20]);
00083     double subsample = atof(argv[21]);
00084 
00085     double __res[] = {0.5, 1, 2};
00086     std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00087     resolutions.push_back(res_max);
00088 
00089     lslgeneric::NDTMatcherP2D matcherP2D(resolutions);
00090     matcherP2D.ITR_MAX = itr_max;
00091     matcherP2D.subsample_size = subsample;
00092     bool ret = matcherP2D.match(cloud_fixed,cloud_offset,Tout);
00093     
00094     tend = getDoubleTime();
00095     Tout = Tout*Tinit;
00096     Eigen::Matrix<double, 4, 4> m = Tout.matrix();
00097     std::cout<<tend-tnow<<", "<<m(0,0)<<", "<<m(0,1)<<", "<<m(0,2)<<", "<<m(0,3)<<", "
00098         <<m(1,0)<<", "<<m(1,1)<<", "<<m(1,2)<<", "<<m(1,3)<<", "
00099         <<m(2,0)<<", "<<m(2,1)<<", "<<m(2,2)<<", "<<m(2,3)<<", "
00100         <<m(3,0)<<", "<<m(3,1)<<", "<<m(3,2)<<", "<<m(3,3)<<std::endl;
00101 
00102 #ifdef DODBG
00103     std::cerr<<"Transform: \n"<<Tout.matrix()<<std::endl;
00104     std::cerr<<"Time: "<<tend-tnow<<std::endl;
00105     //rgb point clouds
00106     //pcl::PointCloud<pcl::PointXYZ> cloud_trans = cloud_offset;
00107     lslgeneric::transformPointCloudInPlace(Tout,cloud_trans);
00108     pcl::PointCloud<pcl::PointXYZRGB> cloud_comb;
00109     pcl::PointXYZRGB red(255,0,0);
00110     for(int i=0; i<cloud_fixed.points.size(); ++i ) {
00111         red.x = cloud_fixed.points[i].x;
00112         red.y = cloud_fixed.points[i].y;
00113         red.z = cloud_fixed.points[i].z;
00114         cloud_comb.points.push_back(red);
00115     }
00116     pcl::PointXYZRGB green(0,200,0);
00117     for(int i=0; i<cloud_offset.points.size(); ++i ) {
00118         green.x = cloud_offset.points[i].x;
00119         green.y = cloud_offset.points[i].y;
00120         green.z = cloud_offset.points[i].z;
00121         cloud_comb.points.push_back(green);
00122     }
00123     pcl::PointXYZRGB blue(10,20,200);
00124     for(int i=0; i<cloud_trans.points.size(); ++i ) {
00125         blue.x = cloud_trans.points[i].x;
00126         blue.y = cloud_trans.points[i].y;
00127         blue.z = cloud_trans.points[i].z;
00128         cloud_comb.points.push_back(blue);
00129     }
00130     cloud_comb.width=1;
00131     cloud_comb.height=cloud_comb.points.size();
00132     cloud_comb.is_dense = false;
00133     pcl::io::savePCDFileBinary ("test_pcd.pcd", cloud_comb);
00134     pcl::io::savePCDFile ("test_pcd_fixed.pcd", cloud_fixed);
00135     pcl::io::savePCDFile ("test_pcd_trans.pcd", cloud_trans);
00136 #endif
00137 
00138     return 0;
00139 }


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