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
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
00034
00035 #ifdef FULLTESTER
00036 std::string fname_str = argv[1];
00037 int nclouds = atoi(argv[2]);
00038
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
00082 lslgeneric::NDTMap<pcl::PointXYZ> nd(&tr);
00083 nd.loadPointCloud(cloud);
00084
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
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
00123 lslgeneric::writeToVRML(f,cloud,Eigen::Vector3d(0,1,0));
00124
00125 lslgeneric::writeToVRML(f,cloud2,Eigen::Vector3d(1,0,0));
00126
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