test_ndt_histogram.cc
Go to the documentation of this file.
00001 #include <ndt_histogram.h>
00002 #include <oc_tree.h>
00003 #include <pointcloud_utils.h>
00004 #include <lazy_grid.h>
00005 
00006 #include "pcl/point_cloud.h"
00007 #include "sensor_msgs/PointCloud2.h"
00008 #include "pcl/io/pcd_io.h"
00009 #include "pcl/features/feature.h"
00010 #include <cstdio>
00011 
00012 #include <fstream>
00013 
00014 //#define FULLTESTER
00015 
00016 using namespace std;
00017 
00018 int
00019 main (int argc, char** argv)
00020 {
00021 
00022     if(argc!=3)
00023     {
00024         cout<<"usage: histTest point_cloud1 point_cloud2\n";
00025         return(-1);
00026     }
00027     pcl::PointCloud<pcl::PointXYZ> cloud, cloud2, cloud3;
00028     pcl::PointCloud<pcl::PointXYZI> outCloud;
00029 
00030     lslgeneric::OctTree<pcl::PointXYZ> tr;
00031     tr.BIG_CELL_SIZE = 2;
00032     tr.SMALL_CELL_SIZE = 0.2;
00033 //    lslgeneric::LazzyGrid tr(0.5);
00034 
00035 #ifdef FULLTESTER
00036     std::string fname_str = argv[1];
00037     int nclouds = atoi(argv[2]);
00038     //lslgeneric::AdaptiveOctTree::MIN_CELL_SIZE = 0.01;
00039 
00040     ofstream logger ("similarity.m");
00041     logger<< "S = [";
00042     struct timeval tv_start, tv_end;
00043     gettimeofday(&tv_start,NULL);
00044     lslgeneric::NDTHistogram<pc::PointXYZ> *array  = new lslgeneric::NDTHistogram<pc::PointXYZ>[nclouds];
00045     for(int i=0; i<nclouds; i++)
00046     {
00047         char cloudname [500];
00048         snprintf(cloudname, 499, "%s%03d.wrl", fname_str.c_str(),i);
00049         cloud = lslgeneric::readVRML(cloudname);
00050         lslgeneric::NDTMap<pc::PointXYZ> nd(&tr);
00051         nd.loadPointCloud(cloud);
00052         nd.computeNDTCells();
00053 
00054         array[i] = lslgeneric::NDTHistogram<pc::PointXYZ>(nd);
00055     }
00056     gettimeofday(&tv_end,NULL);
00057     double avg_build = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00058     avg_build = avg_build/nclouds;
00059     cout<<"building histograms took "<<avg_build<<" msec per scan\n";
00060 
00061     gettimeofday(&tv_start,NULL);
00062     for(int i=0; i<nclouds; i++)
00063     {
00064         for(int j=0; j<nclouds; j++)
00065         {
00066             logger<<array[j].getSimilarity(array[i])<<" ";
00067         }
00068         logger<<";\n";
00069         cout<<" I "<<i<<endl;
00070     }
00071 
00072     gettimeofday(&tv_end,NULL);
00073     double avg_match = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00074     avg_match = avg_match/(nclouds*nclouds);
00075     cout<<"matching histograms took "<<avg_match<<" msec per scan\n";
00076 
00077     logger<<"];\n";
00078 #else
00079     cloud = lslgeneric::readVRML<pcl::PointXYZ>(argv[1]);
00080     cloud2 = lslgeneric::readVRML<pcl::PointXYZ>(argv[2]);
00081     //lslgeneric::NDTMap nd(new lslgeneric::LazzyGrid(5));
00082     lslgeneric::NDTMap<pcl::PointXYZ> nd(&tr);
00083     nd.loadPointCloud(cloud);
00084     //lslgeneric::NDTMap nd2(new lslgeneric::LazzyGrid(5));
00085     lslgeneric::NDTMap<pcl::PointXYZ> nd2(&tr);
00086     nd2.loadPointCloud(cloud2);
00087 
00088     nd.computeNDTCells();
00089     nd2.computeNDTCells();
00090 
00091     lslgeneric::NDTHistogram<pcl::PointXYZ> nh(nd);
00092     lslgeneric::NDTHistogram<pcl::PointXYZ> nh2(nd2);
00093     cout<<"1 =========== \n";
00094     nh.printHistogram(true);
00095     cout<<"2 =========== \n";
00096     nh2.printHistogram(true);
00097 
00098     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;
00099 
00100     nh2.bestFitToHistogram(nh,T,true);
00101     cout<<" ==================== \n Transform R "<<T.rotation()<<"\nt "<<T.translation().transpose()<<endl;
00102 
00103     cout<<"scan similarity is "<<nh2.getSimilarity(nh)<<endl;
00104     cloud3 = lslgeneric::transformPointCloud(T,cloud2);
00105     //lslgeneric::NDTMap nd3(new lslgeneric::LazzyGrid(5));
00106     lslgeneric::NDTMap<pcl::PointXYZ> nd3(&tr);
00107     nd3.loadPointCloud(cloud3);
00108     nd3.computeNDTCells();
00109 
00110     lslgeneric::NDTHistogram<pcl::PointXYZ> nh3(nd3);
00111     cout<<"3 =========== \n";
00112     nh3.printHistogram(true);
00113 
00114 
00115     char fname[50];
00116     snprintf(fname,49,"ndt_map.wrl");
00117     nd.writeToVRML(fname);
00118 
00119     snprintf(fname,49,"histogramRegistered.wrl");
00120     FILE *f = fopen(fname,"w");
00121     fprintf(f,"#VRML V2.0 utf8\n");
00122     //green = target
00123     lslgeneric::writeToVRML(f,cloud,Eigen::Vector3d(0,1,0));
00124     //red = init
00125     lslgeneric::writeToVRML(f,cloud2,Eigen::Vector3d(1,0,0));
00126     //white = final
00127     lslgeneric::writeToVRML(f,cloud3,Eigen::Vector3d(1,1,1));
00128     fclose(f);
00129 
00130     std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > directions = nh.directions;
00131     cout<<"direction = [";
00132     for(int i=0; i<directions.size(); i++)
00133     {
00134         cout<<directions[i].transpose()<<";";
00135     }
00136     cout<<"];\n";
00137 #endif
00138 
00139 
00140 
00141     return (0);
00142 }
00143 
00144 
00145 


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