00001
00002
00003
00004
00005 #include <ndt_matcher_p2d.h>
00006 #include <ndt_matcher_d2d.h>
00007
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
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
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
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
00119
00120
00121 cloud = lslgeneric::readVRML<pcl::PointXYZ>(argv[1]);
00122 cloud_offset = lslgeneric::readVRML<pcl::PointXYZ>(argv[2]);
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
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
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
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++)
00200 {
00201 double xoffset = xmin+i*dx;
00202 for(int q=0; q<N_Y; q++)
00203 {
00204 double yoffset = ymin +q*dy;
00205
00206
00207
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
00219 targetNDT.addPointCloud(so,cloudL);
00220 targetNDT.computeNDTCells();
00221
00222
00223 scores2(i,q) = matcherD2DLocal.scoreNDT_OM(sourceNDT,targetNDT);
00224 scores(i,q) = matcherD2DLocal.scoreNDT( source, targetNDT);
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267 }
00268 }
00269 }
00270
00271
00272 std::cout<< " S = ["<<scores<<"];\n";
00273 std::cout<< " S2 = ["<<scores2<<"];\n";
00274
00275
00276
00277
00278 return 0;
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296 gettimeofday(&tv_reg_start,NULL);
00297 ret = matcherD2D.match(cloud,cloud_offset,Tout);
00298
00299
00300 gettimeofday(&tv_reg_end,NULL);
00301
00302 lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
00303
00304
00305
00306
00307 snprintf(fname,49,"c_offset.pcd");
00308
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
00319
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
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
00349
00350
00351
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
00365
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
00375
00376
00377
00378
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
00387
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
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
00405
00406
00407
00408
00409
00410
00411 gettimeofday(&tv_start,NULL);
00412
00413 bool ret = matcherF2F.match(cloud,cloud_offset,Tout);
00414
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
00434
00435
00436
00437
00438
00439
00440 Eigen::Vector3d out = Tout.rotation().eulerAngles(0,1,2);
00441 logger<<"OUT: "<<out.transpose()<<endl;
00442
00443 logger<<"translation "<<Tout.translation().transpose()<<endl;
00444
00445 snprintf(fname,49,"/home/tsv/ndt_tmp/cloud_offset%05d.wrl",ctr);
00446 lslgeneric::transformPointCloudInPlace(Tout,cloud_offset);
00447
00448
00449
00450
00451
00452
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
00492
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