ndt_registration_test.cc
Go to the documentation of this file.
00001 //#ifndef DO_DEBUG_PROC
00002 //#define DO_DEBUG_PROC
00003 //#endif
00004 
00005 #include <ndt_matcher_p2d.h>
00006 #include <ndt_matcher_d2d.h>
00007 //#include <ndt_matcher_d2d.hh>
00008 #include <ndt_map.h>
00009 #include <oc_tree.h>
00010 #include <pointcloud_utils.h>
00011 
00012 #include "ros/ros.h"
00013 #include "pcl/point_cloud.h"
00014 #include "sensor_msgs/PointCloud2.h"
00015 #include "pcl/io/pcd_io.h"
00016 #include "pcl/features/feature.h"
00017 #include "pcl/registration/icp.h"
00018 #include "pcl/filters/voxel_grid.h"
00019 #include <pcl/filters/passthrough.h>
00020 
00021 #include <cstdio>
00022 #include <Eigen/Eigen>
00023 #include <Eigen/Geometry>
00024 #include <fstream>
00025 
00026 using namespace std;
00027 
00028 
00029 /*
00030 static int ctr = 0;
00031 bool matchICP(pcl::PointCloud<pcl::PointXYZ> &fixed,  pcl::PointCloud<pcl::PointXYZ> &moving,
00032               Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout) {
00033 
00034     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
00035     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
00036     pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(fixed) );
00037     pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(moving) );
00038 
00039     pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
00040     gr1.setLeafSize(0.1,0.1,0.1);
00041     gr2.setLeafSize(0.1,0.1,0.1);
00042 
00043     gr1.setInputCloud(m);
00044     gr2.setInputCloud(f);
00045 
00046     cloud_in->height = 1;
00047     cloud_in->width = cloud_in->points.size();
00048     cloud_out->height = 1;
00049     cloud_out->width = cloud_out->points.size();
00050     cloud_in->is_dense = false;
00051     cloud_out->is_dense = false;
00052 
00053     gr1.filter(*cloud_in);
00054     gr2.filter(*cloud_out);
00055 
00056     pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
00057 
00058     icp.setMaximumIterations(10000);
00059     cout<<"max itr are "<<icp.getMaximumIterations()<<endl;
00060     icp.setInputCloud(cloud_in);
00061     icp.setInputTarget(cloud_out);
00062 
00063     icp.setRANSACOutlierRejectionThreshold (2);
00064     icp.setMaxCorrespondenceDistance(10);
00065     icp.setTransformationEpsilon(0.00001);
00066 //    cout<<"ransac outlier thersh   : "<<icp.getRANSACOutlierRejectionThreshold ()<<endl;
00067 //    cout<<"correspondance max dist : "<<icp.getMaxCorrespondenceDistance() << endl;
00068 //    cout<<"epsilon : "<<icp.getTransformationEpsilon() << endl;
00069     pcl::PointCloud<pcl::PointXYZ> Final;
00070     icp.align(Final);
00071 
00072 
00073 //    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
00074 //      icp.getFitnessScore() << std::endl;
00075 //    std::cout << icp.getFinalTransformation() << std::endl;
00076 
00077     //Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> tTemp;
00078     Tout = (icp.getFinalTransformation()).cast<double>();
00079 
00080     char fname[50];
00081     snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
00082     FILE *fout = fopen(fname,"w");
00083     fprintf(fout,"#VRML V2.0 utf8\n");
00084     lslgeneric::writeToVRML(fout,*cloud_out,Eigen::Vector3d(0,1,0));
00085     lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(1,0,0));
00086     lslgeneric::writeToVRML(fout,*cloud_in,Eigen::Vector3d(1,1,1));
00087     fclose(fout);
00088 
00089     return icp.hasConverged();
00090 
00091 }
00092 */
00093 
00094 int
00095 main (int argc, char** argv)
00096 {
00097 
00098     double roll=0,pitch=0,yaw=0,zoffset=0;
00099 
00100     pcl::PointCloud<pcl::PointXYZ> cloud, cloud_offset, cloud_OFF;
00101     pcl::PCDReader reader;
00102     pcl::PCDWriter writer;
00103     char fname[50];
00104     FILE *fout;
00105     //double __res[] = {0.5};
00106     double __res[] = {0.5, 1, 2, 4};
00107     std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00108     lslgeneric::NDTMatcherP2D<pcl::PointXYZ,pcl::PointXYZ> matcherP2D(resolutions);
00109 
00110     struct timeval tv_start,tv_end,tv_reg_start,tv_reg_end;
00111 
00112     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tin, Tout, Tout2;
00113     Tout.setIdentity();
00114     if(argc == 3)
00115     {
00116 
00117         gettimeofday(&tv_start,NULL);
00118         //we do a single scan to scan registration
00119         //reader.read(argv[1],cloud);
00120         //reader.read(argv[2],cloud_offset);
00121         cloud = lslgeneric::readVRML<pcl::PointXYZ>(argv[1]);
00122         cloud_offset = lslgeneric::readVRML<pcl::PointXYZ>(argv[2]);
00123         /*
00124         Tin =  Eigen::Translation<double,3>(0.5,0.2,0)*
00125             Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitX()) *
00126             Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitY()) *
00127             Eigen::AngleAxis<double>(0.3,Eigen::Vector3d::UnitZ()) ;
00128             */
00129         //cloud_offset = lslgeneric::transformPointCloud(Tin,cloud);
00130         //cloud_offset = cloud;
00131         /*
00132                 snprintf(fname,49,"c_offset.wrl");
00133                 FILE *fout = fopen(fname,"w");
00134                 fprintf(fout,"#VRML V2.0 utf8\n");
00135                 lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(0,1,0));
00136                 lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,0,0));
00137                 fclose(fout);
00138         */
00139         lslgeneric::NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2D(false, false, resolutions);
00140         bool ret = matcherD2D.match2D(cloud,cloud_offset,Tout);
00141 
00142         snprintf(fname,49,"c_offset.wrl");
00143         fout = fopen(fname,"w");
00144         fprintf(fout,"#VRML V2.0 utf8\n");
00145         lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(1,0,0));
00146         lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,1,1));
00147 
00148         lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
00149         std::cout<<Tout.matrix()<<std::endl;
00150         lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(0,1,0));
00151         fclose(fout);
00152 
00153         return 0;
00154         
00155         Eigen::Vector3d sensor_origin;
00156         sensor_origin<<0,0,0;
00157         lslgeneric::NDTMap<pcl::PointXYZ> sourceNDT(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.5));
00158         //sourceNDT.loadPointCloud(cloud);
00159         sourceNDT.addPointCloud(sensor_origin,cloud);
00160         sourceNDT.computeNDTCells();
00161         Tin.setIdentity();
00162 
00163         lslgeneric::NDTMap<pcl::PointXYZ> targetNDT(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.5));
00164         targetNDT.loadPointCloud(cloud_offset);
00165         targetNDT.computeNDTCells();
00166 
00167         matcherD2D.match2D(sourceNDT,targetNDT,Tout);
00168         snprintf(fname,49,"c_offset.wrl");
00169         fout = fopen(fname,"w");
00170         fprintf(fout,"#VRML V2.0 utf8\n");
00171         lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(1,0,0));
00172         lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,1,1));
00173         lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
00174         std::cout<<Tout.matrix()<<std::endl;
00175         lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(0,1,0));
00176         fclose(fout);
00177     }
00178 }
00179 #if 0
00180 std::vector<lslgeneric::NDTCell<pcl::PointXYZ>*> source = sourceNDT.pseudoTransformNDT(Tin);
00181 int N_X=40;
00182 int N_Y=40;
00183 double xmin = -1, ymin = -1, yawmin = -0.5;
00184 double dx = 0.05, dy = 0.05, dyaw = 0.01;
00185 //Eigen::MatrixXd H1(N_X,N_Y), H2(N_X,N_Y);
00186 Eigen::MatrixXd scores(N_X,N_Y);
00187 Eigen::MatrixXd scores2(N_X,N_Y);
00188 Eigen::MatrixXd gX(N_X,N_Y), gY(N_X,N_Y);
00189 scores.setZero();
00190 gX.setZero();
00191 gY.setZero();
00192 int N_SUCC = 0;
00193 std::vector<double> et;
00194 std::vector<double> er;
00195 #pragma omp parallel
00196 {
00197     lslgeneric::NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2DLocal(false, false, resolutions);
00198     #pragma omp for
00199     for(int i=0; i<N_X; i++)   //xoffset=xmin;xoffset<xmin+N_X*dx;xoffset+=dx) {
00200     {
00201         double xoffset = xmin+i*dx;
00202         for(int q=0; q<N_Y; q++)   //yoffset=ymin;yoffset<ymin+N_Y*dy;yoffset+=dy) {
00203         {
00204             double yoffset = ymin +q*dy;
00205 //              for(int j=0; j<N_Y; j++) { //yoffset=ymin;yoffset<ymin+N_Y*dy;yoffset+=dy) {
00206 //                  double yawoffset = yawmin +j*dyaw;
00207 //                  double xoffset = -0.5, yoffset=0.3, yawoffset=-0.2;
00208             double yawoffset=0;
00209             Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tloc =
00210                 Eigen::Translation<double,3>(xoffset,yoffset,0)*
00211                 Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitX()) *
00212                 Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitY()) *
00213                 Eigen::AngleAxis<double>(yawoffset,Eigen::Vector3d::UnitZ()) ;
00214 
00215             Eigen::Vector3d so = Tloc*sensor_origin;
00216             pcl::PointCloud<pcl::PointXYZ> cloudL = lslgeneric::transformPointCloud<pcl::PointXYZ>(Tloc,cloud_offset);
00217             lslgeneric::NDTMap<pcl::PointXYZ> targetNDT(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.5));
00218             //targetNDT.loadPointCloud(cloudL);
00219             targetNDT.addPointCloud(so,cloudL);
00220             targetNDT.computeNDTCells();
00221             //matcherD2DLocal.match(sourceNDT,targetNDT,Tout);
00222             //Tout = Tloc*Tout;
00223             scores2(i,q) = matcherD2DLocal.scoreNDT_OM(sourceNDT,targetNDT);
00224             scores(i,q) = matcherD2DLocal.scoreNDT( source, targetNDT);
00225             //std::cout<<"score: "<<d<<std::endl;
00226             /*
00227                             lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloudL);
00228                             snprintf(fname,49,"c_offset_l.wrl");
00229                             fout = fopen(fname,"w");
00230                             fprintf(fout,"#VRML V2.0 utf8\n");
00231                             lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(0,1,0));
00232                             lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloudL,Eigen::Vector3d(1,0,0));
00233                             fclose(fout);
00234                             cout<<xoffset<<" "<<yoffset<<" "<<yawoffset<<std::endl;
00235                             cout<<"E translation "<<Tout.translation().transpose()
00236                                 <<" (norm) "<<Tout.translation().norm()<<endl;
00237                             cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
00238                                 <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<endl;
00239 
00240                             et.push_back(Tout.translation().norm());
00241                             er.push_back(Tout.rotation().eulerAngles(0,1,2).norm());
00242                             if(Tout.translation().norm() < 0.2 && Tout.rotation().eulerAngles(0,1,2).norm() < 0.087) {
00243                                 cout<<"OK\n";
00244                                 N_SUCC++;
00245                             }
00246             */
00247 
00248             /*
00249             Eigen::MatrixXd score_gradient(6,1);
00250             Eigen::MatrixXd Hessian(6,6);
00251 
00252             std::vector<lslgeneric::NDTCell<pcl::PointXYZ>*> source = sourceNDT.pseudoTransformNDT(Tlocal);
00253 
00254             scores(i,j) = matcherD2D.derivativesNDT( source, targetNDT, score_gradient, Hessian, false);
00255             gX(i,j) = score_gradient(0,0);
00256             gY(i,j) = score_gradient(1,0);
00257             //          Eigen::Matrix<double,6,1>  pose_increment_v;
00258             //          pose_increment_v = -Hessian.ldlt().solve(score_gradient);
00259             //          H1(i,j) = pose_increment_v(0);
00260             //          H2(i,j) = pose_increment_v(1);
00261 
00262             for(int q =0; q<source.size(); ++q) {
00263             delete source[q];
00264             }
00265              */
00266 //              }
00267         }
00268     }
00269 }
00270 
00271 //          cout<<"N_SUCC = "<<N_SUCC<<std::endl;
00272 std::cout<< " S = ["<<scores<<"];\n";
00273 std::cout<< " S2 = ["<<scores2<<"];\n";
00274 //      std::cout<< " gX = ["<<gX<<"];\n";
00275 //      std::cout<< " gY = ["<<gY<<"];\n";
00276 //      std::cout<< " H1 = ["<<H1<<"];\n";
00277 //      std::cout<< " H2 = ["<<H2<<"];\n";
00278 return 0;
00279 /*      pcl::PointCloud<pcl::PointXYZ> cloud1, cloud_offset1;
00280         for(int i=0; i<cloud.points.size(); i++) {
00281             double d = sqrt(pow(cloud.points[i].x,2) + pow(cloud.points[i].y,2) +pow(cloud.points[i].z,2));
00282             if(d<3 && d>1) {
00283                 cloud1.points.push_back(cloud.points[i]);
00284             }
00285         }
00286         for(int i=0; i<cloud_offset.points.size(); i++) {
00287             double d = sqrt(pow(cloud_offset.points[i].x,2) + pow(cloud_offset.points[i].y,2) +pow(cloud_offset.points[i].z,2));
00288             if(d<3 && d>1) {
00289                 cloud_offset1.points.push_back(cloud_offset.points[i]);
00290             }
00291         }
00292 */
00293 //      bool ret = matcherP2D.match(cloud,cloud_offset,Tout2);
00294 
00295 //      lslgeneric::NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2D(false, false, resolutions);
00296 gettimeofday(&tv_reg_start,NULL);
00297 ret = matcherD2D.match(cloud,cloud_offset,Tout);
00298 //bool ret = matcherD2D.match(targetNDT,sourceNDT,Tout);
00299 //bool ret = matcherD2D.match(sourceNDT,targetNDT,Tout);
00300 gettimeofday(&tv_reg_end,NULL);
00301 
00302 lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
00303 //      cout<<"translation "<<Tout.translation().transpose()<<endl;
00304 //      cout<<"euler: "<<Tout.rotation().eulerAngles(0,1,2).transpose()<<endl;
00305 
00306 // cloud_offset += cloud;
00307 snprintf(fname,49,"c_offset.pcd");
00308 // writer.write(fname, cloud_offset);
00309 
00310 
00311 snprintf(fname,49,"c_offset.wrl");
00312 fout = fopen(fname,"w");
00313 fprintf(fout,"#VRML V2.0 utf8\n");
00314 lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(0,1,0));
00315 lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,0,0));
00316 fclose(fout);
00317 
00318 //      cout<<"initial registration done, saved into file c_offset.pcd\n";
00319 //      cout<<"make sure to check that initial registration was successful!\n";
00320 
00321 gettimeofday(&tv_end,NULL);
00322 double tim = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00323 double tim2 = (tv_reg_end.tv_sec-tv_reg_start.tv_sec)*1000.+(tv_reg_end.tv_usec-tv_reg_start.tv_usec)/1000.;
00324 
00325 //      cout<<"OVERALL TIME: "<<tim<<" REG TIME "<<tim2<<endl;
00326 return 0;
00327 }
00328 #if 0
00329 if(!(argc == 2 || succ))
00330 {
00331     cout<<"usage: ./test_ndt point_cloud1 [point_cloud2]\n";
00332     return -1;
00333 }
00334 
00335 cloud = lslgeneric::readVRML(argv[1]);
00336 
00337         char fname[50];
00338         FILE *fout;
00339         if(!succ)
00340 {
00341     cloud_offset = cloud;
00342 }
00343 else
00344 {
00345     cloud_offset = cloud_OFF;
00346 }
00347 
00348 //matcherF2F.generateScoreDebug("/home/tsv/ndt_tmp/scoresF2F.m",cloud,cloud_offset);
00349 //matcherP2F.generateScoreDebug("/home/tsv/ndt_tmp/scoresP2F.m",cloud,cloud_offset);
00350 //cout<<"Generated scores\n";
00351 //return 0;
00352 
00353 std::vector<double> times;
00354 std::vector<int> success;
00355 std::vector<double> et;
00356 std::vector<double> er;
00357 
00358 roll = 0;
00359        pitch =0;
00360               yaw = 40*M_PI/180;
00361                     xoffset = -2;
00362                               yoffset = -1.;
00363                                         zoffset =0;
00364                                                 //for(roll = -2*M_PI/5;roll<M_PI/2;roll+=M_PI/5) {
00365                                                 //for(pitch = -2*M_PI/5;pitch<M_PI/2;pitch+=M_PI/5) {
00366                                                 for(yaw = -30*M_PI/180; yaw<=35*M_PI/180; yaw+=10*M_PI/180)
00367 {
00368     logger2<<"%";
00369     for(xoffset=-1.5; xoffset<=1.5; xoffset+=0.5)
00370     {
00371         for(yoffset=-1.5; yoffset<=1.5; yoffset+=0.5)
00372         {
00373             /*
00374             for(yaw = -5*M_PI/180;yaw<=15*M_PI/180;yaw+=10*M_PI/180) {
00375             for(xoffset=-0.1;xoffset<=0.5;xoffset+=0.5) {
00376             for(yoffset=0;yoffset<=0.5;yoffset+=0.5) {
00377             */
00378             //for(zoffset = -1;zoffset<1;zoffset+=0.5) {
00379 
00380 
00381             Tin =  Eigen::Translation<double,3>(xoffset,yoffset,zoffset)*
00382                    Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX()) *
00383                    Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
00384                    Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) ;
00385 
00386             //cout<<"translation \n"<<Tin.translation().transpose()<<endl;
00387             //cout<<"rotation \n"<<Tin.rotation()<<endl;
00388 
00389             cout<<roll<<" "<<pitch<<" "<<yaw<<" "<<xoffset<<" "<<yoffset<<" "<<zoffset<<endl;
00390             logger<<roll<<" "<<pitch<<" "<<yaw<<" "<<xoffset<<" "<<yoffset<<" "<<zoffset<<endl;
00391 
00392             Eigen::Vector3d out = Tin.rotation().eulerAngles(0,1,2);
00393             //cout<<"euler: "<<out<<endl;
00394 
00395             if(!succ)
00396             {
00397                 cloud_offset = lslgeneric::transformPointCloud(Tin,cloud);
00398             }
00399             else
00400             {
00401                 cloud_offset = lslgeneric::transformPointCloud(Tin,cloud_OFF);
00402             }
00403 
00404             /*snprintf(fname,49,"/home/tsv/ndt_tmp/cloud_init%05d.wrl",ctr);
00405             fout = fopen(fname,"w");
00406             fprintf(fout,"#VRML V2.0 utf8\n");
00407             lslgeneric::writeToVRML(fout,cloud_offset,Eigen::Vector3d(1,0,0));
00408             lslgeneric::writeToVRML(fout,cloud,Eigen::Vector3d(0,1,0));
00409             fclose(fout);
00410             */
00411             gettimeofday(&tv_start,NULL);
00412             //bool ret = matcherP2F.match(cloud,cloud_offset,Tout);
00413             bool ret = matcherF2F.match(cloud,cloud_offset,Tout);
00414             //bool ret = matchICP(cloud,cloud_offset,Tout);
00415             gettimeofday(&tv_end,NULL);
00416 
00417             double tim = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00418             logger<<"TIME: "<<tim<<endl;
00419             C_TIME+=tim;
00420 
00421             if(!ret)
00422             {
00423                 logger<<">>>>>>> NO SOLUTION <<<<<<<<<\n";
00424                 cout<<"NO SOLUTION\n";
00425                 logger2<<"N ";
00426                 N_FAIL++;
00427                 times.push_back(tim);
00428                 et.push_back(Tout.translation().norm());
00429                 er.push_back(Tout.rotation().eulerAngles(0,1,2).norm());
00430             }
00431             else
00432             {
00433                 /*logger<<"Input Transform: "<<endl;
00434                 logger<<"translation "<<Tin.translation()<<endl;
00435                 logger<<"rotation "<<Tin.rotation()<<endl;
00436                 logger<<"translation "<<Tout.translation()<<endl;
00437                 logger<<"rotation "<<Tout.rotation()<<endl;
00438                 */
00439                 //lslgeneric::transformPointCloudInPlace(Tout,cloud_offset);
00440                 Eigen::Vector3d out = Tout.rotation().eulerAngles(0,1,2);
00441                 logger<<"OUT: "<<out.transpose()<<endl;
00442                 //cout<<"OUT: "<<out<<endl;
00443                 logger<<"translation "<<Tout.translation().transpose()<<endl;
00444                 //cout<<"translation "<<Tout.translation()<<endl;
00445                 snprintf(fname,49,"/home/tsv/ndt_tmp/cloud_offset%05d.wrl",ctr);
00446                 lslgeneric::transformPointCloudInPlace(Tout,cloud_offset);
00447 
00448                 /*fout = fopen(fname,"w");
00449                 fprintf(fout,"#VRML V2.0 utf8\n");
00450                 lslgeneric::writeToVRML(fout,cloud_offset,Eigen::Vector3d(1,0,0));
00451                 lslgeneric::writeToVRML(fout,cloud,Eigen::Vector3d(0,1,0));
00452                 fclose(fout);
00453                 */
00454                 ctr++;
00455 
00456                 Tout = Tin*Tout;
00457                 logger<<"E translation "<<Tout.translation().transpose()
00458                 <<" (norm) "<<Tout.translation().norm()<<endl;
00459                 logger<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
00460                 <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<endl;
00461 
00462                 times.push_back(tim);
00463                 et.push_back(Tout.translation().norm());
00464                 er.push_back(Tout.rotation().eulerAngles(0,1,2).norm());
00465 
00466                 if(Tout.translation().norm() < 0.2 && Tout.rotation().eulerAngles(0,1,2).norm() < 0.087)
00467                 {
00468                     logger<<"OK\n";
00469                     logger2<<"O ";
00470                     cout<<"OK\n";
00471                     N_SUCC++;
00472                     success.push_back(0);
00473                 }
00474                 else if(Tout.translation().norm() < 1 && Tout.rotation().eulerAngles(0,1,2).norm() < 0.15)
00475                 {
00476                     logger<<"ACK\n";
00477                     logger2<<"A ";
00478                     cout<<"ACK\n";
00479                     success.push_back(1);
00480                 }
00481                 else
00482                 {
00483                     logger<<"FAIL\n";
00484                     logger2<<"F ";
00485                     cout<<"FAIL\n";
00486                     N_FAIL++;
00487                     success.push_back(2);
00488                 }
00489 
00490                 return 0;
00491                 //cout<<"E translation "<<Tout.translation()<<endl;
00492                 //cout<<"E rotation "<<Tout.rotation()<<endl;
00493             }
00494             N_TRIALS++;
00495             logger <<"-------------------------------------------\n";
00496         }
00497     }
00498     logger2<<"\n";
00499     logger2.flush();
00500 
00501 
00502 }
00503 //}}}}}
00504 
00505 cout<<"Trials:  "<<N_TRIALS<<endl;
00506 cout<<"Success: "<<N_SUCC<<"  Rate = "<<(double)N_SUCC/(double)N_TRIALS<<endl;
00507 cout<<"Success + Acc : "<<N_TRIALS-N_FAIL<<"  Rate = "<<1-(double)N_FAIL/(double)N_TRIALS<<endl;
00508 cout<<"Fail:    "<<N_FAIL<<endl;
00509 cout<<"Runtime  "<<C_TIME/N_TRIALS<<endl;
00510 
00511 logger2<<"Trials:  "<<N_TRIALS<<endl;
00512 logger2<<"Success: "<<N_SUCC<<"  Rate = "<<(double)N_SUCC/(double)N_TRIALS<<endl;
00513 logger2<<"Success + Acc : "<<N_TRIALS-N_FAIL<<"  Rate = "<<1-(double)N_FAIL/(double)N_TRIALS<<endl;
00514 logger2<<"Fail:    "<<N_FAIL<<endl;
00515 logger2<<"Runtime  "<<C_TIME/N_TRIALS<<endl;
00516 
00517 logger2<<"Times = [";
00518 for(int i=0; i<times.size(); i++)
00519 {
00520     logger2<<times[i]<<" ";
00521 }
00522 logger2<<"];\n";
00523 
00524 logger2<<"ER = [";
00525 for(int i=0; i<er.size(); i++)
00526 {
00527     logger2<<er[i]<<" ";
00528 }
00529 logger2<<"];\n";
00530 
00531 logger2<<"ET = [";
00532 for(int i=0; i<et.size(); i++)
00533 {
00534     logger2<<et[i]<<" ";
00535 }
00536 logger2<<"];\n";
00537 
00538 logger2<<"Succ = [";
00539 for(int i=0; i<success.size(); i++)
00540 {
00541     logger2<<success[i]<<" ";
00542 }
00543 logger2<<"];\n";
00544 
00545 return (0);
00546 #endif
00547 }
00548 #endif
00549 
00550 
00551 


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:30