00001 #include "oc_tree.h"
00002 #include "ndt_cell.h"
00003 #include "lazy_grid.h"
00004 #include "pointcloud_utils.h"
00005 #include "Eigen/Eigen"
00006 #include <fstream>
00007 #include <omp.h>
00008
00009 namespace lslgeneric
00010 {
00011
00012
00013
00014 template <typename PointSource, typename PointTarget>
00015 void NDTMatcherD2D<PointSource,PointTarget>::init(bool _isIrregularGrid,
00016 bool useDefaultGridResolutions, std::vector<double> _resolutions)
00017 {
00018 Jest.setZero();
00019 Jest.block<3,3>(0,0).setIdentity();
00020 Hest.setZero();
00021 Zest.setZero();
00022 ZHest.setZero();
00023
00024 isIrregularGrid = _isIrregularGrid;
00025 if(useDefaultGridResolutions)
00026 {
00027 resolutions.push_back(0.2);
00028 resolutions.push_back(0.5);
00029 resolutions.push_back(1);
00030 resolutions.push_back(2);
00031 }
00032 else
00033 {
00034 resolutions = _resolutions;
00035 }
00036
00037 current_resolution = 0.1;
00038 lfd1 = 1;
00039 lfd2 = 0.05;
00040 ITR_MAX = 30;
00041 DELTA_SCORE = 10e-3*current_resolution;
00042 step_control = true;
00043
00044 }
00045
00046 template <typename PointSource, typename PointTarget>
00047 bool NDTMatcherD2D<PointSource,PointTarget>::match( pcl::PointCloud<PointTarget>& target,
00048 pcl::PointCloud<PointSource>& source,
00049 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T ,
00050 bool useInitialGuess)
00051 {
00052
00053 struct timeval tv_start, tv_end;
00054 struct timeval tv_start0, tv_end0;
00055 double time_load =0, time_match=0, time_combined=0;
00056
00057 gettimeofday(&tv_start0,NULL);
00058
00059
00060 pcl::PointCloud<PointSource> sourceCloud = source;
00061 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Temp, Tinit;
00062 Tinit.setIdentity();
00063 if(useInitialGuess)
00064 {
00065 lslgeneric::transformPointCloudInPlace(T,sourceCloud);
00066 Tinit = T;
00067 }
00068
00069 T.setIdentity();
00070 bool ret = false;
00071
00072 #ifdef DO_DEBUG_PROC
00073 char fname[50];
00074 snprintf(fname,49,"initial_clouds.wrl");
00075 FILE *fout = fopen(fname,"w");
00076 fprintf(fout,"#VRML V2.0 utf8\n");
00077
00078 lslgeneric::writeToVRML(fout,target,Eigen::Vector3d(1,0,0));
00079 lslgeneric::writeToVRML(fout,source,Eigen::Vector3d(0,1,0));
00080 fclose(fout);
00081 #endif
00082
00083 if(isIrregularGrid)
00084 {
00085
00086 OctTree<PointTarget> pr1;
00087 NDTMap<PointTarget> targetNDT( &pr1 );
00088 targetNDT.loadPointCloud( target );
00089 targetNDT.computeNDTCells();
00090
00091 OctTree<PointSource> pr2;
00092 NDTMap<PointSource> sourceNDT( &pr2 );
00093 sourceNDT.loadPointCloud( source );
00094 sourceNDT.computeNDTCells();
00095
00096 ret = this->match( targetNDT, sourceNDT, T );
00097
00098 }
00099 else
00100 {
00101
00102
00103 for(int r_ctr = resolutions.size()-1; r_ctr >=0; r_ctr--)
00104 {
00105
00106 current_resolution = resolutions[r_ctr];
00107
00108 LazyGrid<PointSource> prototypeSource(current_resolution);
00109 LazyGrid<PointTarget> prototypeTarget(current_resolution);
00110
00111 gettimeofday(&tv_start,NULL);
00112 NDTMap<PointTarget> targetNDT( &prototypeTarget );
00113 targetNDT.loadPointCloud( target );
00114 targetNDT.computeNDTCells();
00115
00116 NDTMap<PointSource> sourceNDT( &prototypeSource );
00117 sourceNDT.loadPointCloud( sourceCloud );
00118 sourceNDT.computeNDTCells();
00119 gettimeofday(&tv_end,NULL);
00120
00121 time_load += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00122 Temp.setIdentity();
00123
00124 gettimeofday(&tv_start,NULL);
00125 ret = this->match( targetNDT, sourceNDT, Temp );
00126 lslgeneric::transformPointCloudInPlace(Temp,sourceCloud);
00127 gettimeofday(&tv_end,NULL);
00128
00129 time_match += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00130
00131
00132 T = Temp*T;
00133
00134
00135 #ifdef DO_DEBUG_PROC
00136 std::cout<<"RESOLUTION: "<<current_resolution<<std::endl;
00137 std::cout<<"rotation : "<<Temp.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
00138 std::cout<<"translation: "<<Temp.translation().transpose()<<std::endl;
00139 std::cout<<"--------------------------------------------------------\nOverall Transform:\n";
00140 std::cout<<"rotation : "<<T.rotation().eulerAngles(0,1,2).transpose()<<std::endl;
00141 std::cout<<"translation: "<<T.translation().transpose()<<std::endl;
00142
00143 char fname[50];
00144 snprintf(fname,49,"inner_cloud%lf.wrl",current_resolution);
00145 FILE *fout = fopen(fname,"w");
00146 fprintf(fout,"#VRML V2.0 utf8\n");
00147
00148 std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(Temp);
00149 targetNDT.writeToVRML(fout,Eigen::Vector3d(1,0,0));
00150
00151 for(unsigned int i=0; i<nextNDT.size(); i++)
00152 {
00153 if(nextNDT[i]!=NULL)
00154 {
00155 nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00156 delete nextNDT[i];
00157 }
00158 }
00159 lslgeneric::writeToVRML(fout,sourceCloud,Eigen::Vector3d(0,1,0));
00160 fclose(fout);
00161
00162 snprintf(fname,49,"cloud%lf.wrl",current_resolution);
00163 fout = fopen(fname,"w");
00164 fprintf(fout,"#VRML V2.0 utf8\n");
00165 lslgeneric::writeToVRML(fout,target,Eigen::Vector3d(1,0,0));
00166 lslgeneric::writeToVRML(fout,sourceCloud,Eigen::Vector3d(0,1,0));
00167 fclose(fout);
00168
00169 #endif
00170 }
00171 }
00172 if(useInitialGuess)
00173 {
00174 T = T*Tinit;
00175 }
00176 gettimeofday(&tv_end0,NULL);
00177 time_combined = (tv_end0.tv_sec-tv_start0.tv_sec)*1000.+(tv_end0.tv_usec-tv_start0.tv_usec)/1000.;
00178 std::cout<<"load: "<<time_load<<" match "<<time_match<<" combined "<<time_combined<<std::endl;
00179 return ret;
00180 }
00181
00182 #if 0
00183 template <typename PointSource, typename PointTarget>
00184 bool NDTMatcherD2D<PointSource,PointTarget>::matchGSL( NDTMap<PointTarget>& targetNDT,
00185 NDTMap<PointSource>& sourceNDT,
00186 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T ,
00187 bool useInitialGuess)
00188 {
00189
00190
00191 bool convergence = false;
00192
00193 double DELTA_SCORE = 10e-4*current_resolution;
00194
00195 double NORM_MAX = current_resolution, ROT_MAX = M_PI/10;
00196 int itr_ctr = 0;
00197 double step_size = 1;
00198 Eigen::Matrix<double,6,1> pose_increment_v, scg;
00199 Eigen::MatrixXd Hessian(6,6), score_gradient(6,1);
00200
00201 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR;
00202 Eigen::Vector3d transformed_vec, mean;
00203 bool ret = true;
00204 if(!useInitialGuess)
00205 {
00206 T.setIdentity();
00207 }
00208
00209 std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00210
00211 while(!convergence)
00212 {
00213
00214 TR.setIdentity();
00215 Hessian.setZero();
00216 score_gradient.setZero();
00217
00218 double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,true);
00219 scg = score_gradient;
00220 pose_increment_v = Hessian.ldlt().solve(-score_gradient);
00221 double dginit = pose_increment_v.dot(scg);
00222
00223 TR.setIdentity();
00224 TR = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00225 Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00226 Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00227 Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00228
00229 std::cout<<"incr= ["<<pose_increment_v.transpose()<<"]"<<std::endl;
00230 Eigen::Matrix3d t2 = (T.rotation().transpose()*TR.rotation()*T.rotation());
00231
00232 for(unsigned int i=0; i<nextNDT.size(); i++)
00233 {
00234 if(nextNDT[i]!=NULL)
00235 {
00236 if(nextNDT[i]->hasGaussian_)
00237 {
00238 Eigen::Vector3d _mean = nextNDT[i]->getMean();
00239 Eigen::Matrix3d _cov = nextNDT[i]->getCov();
00240 _mean = TR*_mean;
00241 _cov = t2.transpose()*_cov*t2;
00242 nextNDT[i]->setMean(_mean);
00243 nextNDT[i]->setCov(_cov);
00244 }
00245 }
00246 }
00247
00248 T = TR*T;
00249 if(itr_ctr>0)
00250 {
00251 convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
00252 }
00253 if(itr_ctr>ITR_MAX)
00254 {
00255 convergence = true;
00256 ret = false;
00257 }
00258 itr_ctr++;
00259 }
00260 for(unsigned int i=0; i<nextNDT.size(); i++)
00261 {
00262 if(nextNDT[i]!=NULL)
00263 delete nextNDT[i];
00264 }
00265 return ret;
00266 }
00267 #endif
00268 template <typename PointSource, typename PointTarget>
00269 bool NDTMatcherD2D<PointSource,PointTarget>::match( NDTMap<PointTarget>& targetNDT,
00270 NDTMap<PointSource>& sourceNDT,
00271 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T ,
00272 bool useInitialGuess)
00273 {
00274
00275
00276 bool convergence = false;
00277
00278 double score_best = INT_MAX;
00279
00280
00281 int itr_ctr = 0;
00282
00283 double step_size = 1;
00284 Eigen::Matrix<double,6,1> pose_increment_v, scg;
00285 Eigen::MatrixXd Hessian(6,6), score_gradient(6,1);
00286
00287 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR, Tbest;
00288 Eigen::Vector3d transformed_vec, mean;
00289 bool ret = true;
00290 if(!useInitialGuess)
00291 {
00292 T.setIdentity();
00293 }
00294 Tbest = T;
00295
00296
00297 Eigen::Array<double,6,1> weights;
00298 std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00299
00300
00301 while(!convergence)
00302 {
00303 TR.setIdentity();
00304 Hessian.setZero();
00305 score_gradient.setZero();
00306
00307 double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,true);
00308
00309 scg = score_gradient;
00310
00311 if(score_here < score_best)
00312 {
00313 Tbest = T;
00314 score_best = score_here;
00315
00316 }
00317
00318
00319 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol (Hessian);
00320 Eigen::Matrix<double,6,1> evals = Sol.eigenvalues().real();
00321 double minCoeff = evals.minCoeff();
00322 double maxCoeff = evals.maxCoeff();
00323 if(minCoeff < 0)
00324 {
00325 Eigen::Matrix<double,6,6> evecs = Sol.eigenvectors().real();
00326 double regularizer = score_gradient.norm();
00327 regularizer = regularizer + minCoeff > 0 ? regularizer : 0.001*maxCoeff - minCoeff;
00328
00329 Eigen::Matrix<double,6,1> reg;
00330
00331 reg<<regularizer,regularizer,regularizer,regularizer,regularizer,regularizer;
00332 evals += reg;
00333 Eigen::Matrix<double,6,6> Lam;
00334 Lam = evals.asDiagonal();
00335 Hessian = evecs*Lam*(evecs.transpose());
00336
00337 }
00338
00339
00340
00341 if (score_gradient.norm()<= DELTA_SCORE)
00342 {
00343
00344
00345 if(score_here > score_best)
00346 {
00347
00348 T = Tbest;
00349 }
00350
00351 for(unsigned int i=0; i<nextNDT.size(); i++)
00352 {
00353 if(nextNDT[i]!=NULL)
00354 delete nextNDT[i];
00355 }
00356
00357 return true;
00358 }
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377 pose_increment_v = -Hessian.ldlt().solve(score_gradient);
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406 double dginit = pose_increment_v.dot(scg);
00407 if(dginit > 0)
00408 {
00409
00410
00411
00412
00413 if(score_here > score_best)
00414 {
00415
00416 T = Tbest;
00417 }
00418 for(unsigned int i=0; i<nextNDT.size(); i++)
00419 {
00420 if(nextNDT[i]!=NULL)
00421 delete nextNDT[i];
00422 }
00423
00424 return true;
00425 }
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466 if(step_control) {
00467 step_size = lineSearchMT(pose_increment_v,nextNDT,targetNDT);
00468 } else {
00469 step_size = 1;
00470 }
00471 pose_increment_v = step_size*pose_increment_v;
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481 TR.setIdentity();
00482 TR = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00483 Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00484 Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00485 Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00486
00487
00488
00489 T = TR*T;
00490
00491
00492
00493 for(unsigned int i=0; i<nextNDT.size(); i++)
00494 {
00495
00496 Eigen::Vector3d meanC = nextNDT[i]->getMean();
00497 Eigen::Matrix3d covC = nextNDT[i]->getCov();
00498 meanC = TR*meanC;
00499 covC = TR.rotation()*covC*TR.rotation().transpose();
00500 nextNDT[i]->setMean(meanC);
00501 nextNDT[i]->setCov(covC);
00502 }
00503
00504 if(itr_ctr>0)
00505 {
00506 convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
00507
00508 }
00509 if(itr_ctr>ITR_MAX)
00510 {
00511 convergence = true;
00512 ret = false;
00513 }
00514 itr_ctr++;
00515
00516
00517 }
00518
00519
00520
00521 score_gradient.setZero();
00522 double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,false);
00523 if(score_here > score_best)
00524 {
00525
00526 T = Tbest;
00527 }
00528 for(unsigned int i=0; i<nextNDT.size(); i++)
00529 {
00530 if(nextNDT[i]!=NULL)
00531 delete nextNDT[i];
00532 }
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554 return ret;
00555 }
00556
00557
00558 #if 0
00559 template <typename PointSource, typename PointTarget>
00560 bool NDTMatcherD2D<PointSource,PointTarget>::match_old( NDTMap<PointTarget>& targetNDT,
00561 NDTMap<PointSource>& sourceNDT,
00562 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T ,
00563 bool useInitialGuess)
00564 {
00565
00566
00567 bool convergence = false;
00568
00569 double DELTA_SCORE = 10e-3*current_resolution;
00570 double score_best = INT_MAX;
00571
00572
00573 int itr_ctr = 0;
00574
00575 double step_size = 1;
00576 Eigen::Matrix<double,6,1> pose_increment_v, scg;
00577 Eigen::MatrixXd Hessian(6,6), score_gradient(6,1);
00578
00579 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR, Tbest;
00580 Eigen::Vector3d transformed_vec, mean;
00581 bool ret = true;
00582 if(!useInitialGuess)
00583 {
00584 T.setIdentity();
00585 }
00586 Tbest = T;
00587
00588
00589 Eigen::Array<double,6,1> weights;
00590
00591 while(!convergence)
00592 {
00593 std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00594 TR.setIdentity();
00595 Hessian.setZero();
00596 score_gradient.setZero();
00597
00598 double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,true);
00599
00600 scg = score_gradient;
00601
00602 if(score_here < score_best)
00603 {
00604 Tbest = T;
00605 score_best = score_here;
00606
00607 }
00608
00609
00610 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol (Hessian);
00611 Eigen::Matrix<double,6,1> evals = Sol.eigenvalues().real();
00612 double minCoeff = evals.minCoeff();
00613 double maxCoeff = evals.maxCoeff();
00614 if(minCoeff < 0)
00615 {
00616 Eigen::Matrix<double,6,6> evecs = Sol.eigenvectors().real();
00617 double regularizer = score_gradient.norm();
00618 regularizer = regularizer + minCoeff > 0 ? regularizer : 0.001*maxCoeff - minCoeff;
00619
00620 Eigen::Matrix<double,6,1> reg;
00621
00622 reg<<regularizer,regularizer,regularizer,regularizer,regularizer,regularizer;
00623 evals += reg;
00624 Eigen::Matrix<double,6,6> Lam;
00625 Lam = evals.asDiagonal();
00626 Hessian = evecs*Lam*(evecs.transpose());
00627 std::cerr<<"regularizing\n";
00628 }
00629
00630
00631
00632 if (score_gradient.norm()<= DELTA_SCORE)
00633 {
00634
00635 std::cout<<"\%gradient vanished\n";
00636 if(score_here > score_best)
00637 {
00638 std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
00639 T = Tbest;
00640 }
00641
00642 for(unsigned int i=0; i<nextNDT.size(); i++)
00643 {
00644 if(nextNDT[i]!=NULL)
00645 delete nextNDT[i];
00646 }
00647
00648 return true;
00649 }
00650
00651
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668 pose_increment_v = -Hessian.ldlt().solve(score_gradient);
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695
00696
00697 double dginit = pose_increment_v.dot(scg);
00698 if(dginit > 0)
00699 {
00700
00701
00702 std::cout<<"\%can't decrease in this direction any more, done \n";
00703
00704 if(score_here > score_best)
00705 {
00706 std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
00707 T = Tbest;
00708 }
00709 for(unsigned int i=0; i<nextNDT.size(); i++)
00710 {
00711 if(nextNDT[i]!=NULL)
00712 delete nextNDT[i];
00713 }
00714 return true;
00715 }
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756 step_size = lineSearchMT(pose_increment_v,nextNDT,targetNDT);
00757
00758 pose_increment_v = step_size*pose_increment_v;
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768 TR.setIdentity();
00769 TR = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00770 Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00771 Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00772 Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00773
00774
00775
00776 T = TR*T;
00777
00778
00779
00780 for(unsigned int i=0; i<nextNDT.size(); i++)
00781 {
00782 if(nextNDT[i]!=NULL)
00783 delete nextNDT[i];
00784 }
00785
00786 if(itr_ctr>0)
00787 {
00788 convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
00789
00790 }
00791 if(itr_ctr>ITR_MAX)
00792 {
00793 convergence = true;
00794 ret = false;
00795 }
00796 itr_ctr++;
00797
00798
00799 }
00800
00801 std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00802 score_gradient.setZero();
00803 double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,false);
00804 if(score_here > score_best)
00805 {
00806 std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
00807 T = Tbest;
00808 }
00809 for(unsigned int i=0; i<nextNDT.size(); i++)
00810 {
00811 if(nextNDT[i]!=NULL)
00812 delete nextNDT[i];
00813 }
00814
00815
00816
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826
00827
00828
00829
00830
00831
00832
00833
00834
00835 return ret;
00836 }
00837 #endif
00838
00839
00840 template <typename PointSource, typename PointTarget>
00841 bool NDTMatcherD2D<PointSource,PointTarget>::update_gradient_hessian_local(
00842 Eigen::MatrixXd &score_gradient,
00843 Eigen::MatrixXd &Hessian,
00844 const Eigen::Vector3d & x,
00845 const Eigen::Matrix3d & B,
00846 const double &likelihood,
00847 const Eigen::Matrix<double,3,6> &_Jest,
00848 const Eigen::Matrix<double,18,6> &_Hest,
00849 const Eigen::Matrix<double,3,18> &_Zest,
00850 const Eigen::Matrix<double,18,18> &_ZHest,
00851 bool computeHessian)
00852 {
00853
00854
00855
00856 Eigen::Matrix<double,6,1> _xtBJ, _xtBZBx, _Q;
00857
00858 Eigen::Matrix<double,6,6> _xtBZBJ, _xtBH, _xtBZBZBx, _xtBZhBx;
00859 Eigen::Matrix<double,1,3> _TMP1, _xtB;
00860
00861 _xtBJ.setZero();
00862 _xtBZBx.setZero();
00863 _Q.setZero();
00864 _xtBZBJ.setZero();
00865 _xtBH.setZero();
00866 _xtBZBZBx.setZero();
00867 _xtBZhBx.setZero();
00868 _TMP1.setZero();
00869 _xtB.setZero();
00870
00871 _xtB = x.transpose()*B;
00872 _xtBJ = _xtB*_Jest;
00873
00874 for(unsigned int i=0; i<6; i++)
00875 {
00876 _TMP1 = _xtB*_Zest.block<3,3>(0,3*i)*B;
00877 _xtBZBx(i) = _TMP1*x;
00878 if(computeHessian)
00879 {
00880 _xtBZBJ.col(i) = (_TMP1*_Jest).transpose();
00881 for(unsigned int j=0; j<6; j++)
00882 {
00883 _xtBH(i,j) = _xtB*_Hest.block<3,1>(3*i,j);
00884 _xtBZBZBx(i,j) = _TMP1*_Zest.block<3,3>(0,3*j)*B*x;
00885 _xtBZhBx(i,j) = _xtB*_ZHest.block<3,3>(3*i,3*j)*B*x;
00886 }
00887 }
00888 }
00889 _Q = 2*_xtBJ-_xtBZBx;
00890 double factor = -(lfd2/2)*likelihood;
00891 score_gradient += _Q*factor;
00892
00893 if(computeHessian)
00894 {
00895 Hessian += factor*(2*_Jest.transpose()*B*_Jest+2*_xtBH -_xtBZhBx -2*_xtBZBJ.transpose()
00896 -2*_xtBZBJ +_xtBZBZBx +_xtBZBZBx.transpose() -lfd2*_Q*_Q.transpose()/2 );
00897
00898 }
00899 return true;
00900 }
00901
00902 template <typename PointSource, typename PointTarget>
00903 bool NDTMatcherD2D<PointSource,PointTarget>::update_gradient_hessian(
00904 Eigen::MatrixXd &score_gradient,
00905 Eigen::MatrixXd &Hessian,
00906 const Eigen::Vector3d & x,
00907 const Eigen::Matrix3d & B,
00908 const double &likelihood,
00909 bool computeHessian)
00910 {
00911
00912
00913
00914
00915
00916
00917
00918
00919 Eigen::MatrixXd Hloc(6,6), sg(6,1);
00920
00921 Hloc.setZero();
00922 sg.setZero();
00923 xtBJ.setZero();
00924 xtBZBx.setZero();
00925 Q.setZero();
00926 JtBJ.setZero();
00927 xtBZBJ.setZero();
00928 xtBH.setZero();
00929 xtBZBZBx.setZero();
00930 xtBZhBx.setZero();
00931 TMP1.setZero();
00932 xtB.setZero();
00933
00934 xtB = x.transpose()*B;
00935 xtBJ = xtB*Jest;
00936
00937 for(unsigned int i=0; i<6; i++)
00938 {
00939 TMP1 = xtB*Zest.block<3,3>(0,3*i)*B;
00940 xtBZBx(i) = TMP1*x;
00941 if(computeHessian)
00942 {
00943 xtBZBJ.col(i) = (TMP1*Jest).transpose();
00944 for(unsigned int j=0; j<6; j++)
00945 {
00946 xtBH(i,j) = xtB*Hest.block<3,1>(3*i,j);
00947 xtBZBZBx(i,j) = TMP1*Zest.block<3,3>(0,3*j)*B*x;
00948 xtBZhBx(i,j) = xtB*ZHest.block<3,3>(3*i,3*j)*B*x;
00949 }
00950 }
00951 }
00952 Q = 2*xtBJ-xtBZBx;
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
00967 double factor = -(lfd2/2)*likelihood;
00968 sg = Q*factor;
00969
00970 score_gradient += sg;
00971
00972 if(computeHessian)
00973 {
00974 Hloc= factor*(2*Jest.transpose()*B*Jest+2*xtBH -xtBZhBx -2*xtBZBJ.transpose()
00975 -2*xtBZBJ +xtBZBZBx +xtBZBZBx.transpose() -lfd2*Q*Q.transpose()/2 );
00976 Hessian += Hloc;
00977
00978 }
00979
00980
00981
00982
00983
00984
00985
00986
00987 return true;
00988
00989 }
00990
00991
00992 template <typename PointSource, typename PointTarget>
00993 void NDTMatcherD2D<PointSource,PointTarget>::computeDerivativesLocal(Eigen::Vector3d &x, Eigen::Matrix3d C1,
00994 Eigen::Matrix<double,3,6> &_Jest,
00995 Eigen::Matrix<double,18,6> &_Hest,
00996 Eigen::Matrix<double,3,18> &_Zest,
00997 Eigen::Matrix<double,18,18> &_ZHest,
00998 bool computeHessian)
00999 {
01000
01001 _Jest(0,4) = x(2);
01002 _Jest(0,5) = -x(1);
01003 _Jest(1,3) = -x(2);
01004 _Jest(1,5) = x(0);
01005 _Jest(2,3) = x(1);
01006 _Jest(2,4) = -x(0);
01007
01008 Eigen::Matrix3d myBlock;
01009
01010 myBlock<<
01011 0, -C1(0,2), C1(0,1),
01012 -C1(0,2), -2*C1(1,2), -C1(2,2) + C1(1,1),
01013 C1(0,1), -C1(2,2) + C1(1,1), 2*C1(1,2);
01014 _Zest.block<3,3>(0,9) = myBlock;
01015 myBlock<<
01016 2*C1(0,2), C1(1,2), -C1(0,0) + C1(2,2),
01017 C1(1,2), 0, -C1(0,1),
01018 -C1(0,0) + C1(2,2), -C1(0,1), -2*C1(0,2);
01019 _Zest.block<3,3>(0,12) = myBlock;
01020 myBlock<<
01021 -2*C1(0,1), -C1(1,1) + C1(0,0), -C1(1,2),
01022 -C1(1,1) + C1(0,0), 2*C1(0,1), C1(0,2),
01023 -C1(1,2), C1(0,2), 0;
01024 _Zest.block<3,3>(0,15) = myBlock;
01025
01026 if(computeHessian)
01027 {
01028 Eigen::Vector3d a,b,c,d,e,f;
01029 a<<0,-x(1),-x(2);
01030 b<<0,x(0),0;
01031 c<<0,0,x(0);
01032 d<<-x(0),0,-x(2);
01033 e<<0,0,x(1);
01034 f<<-x(0),-x(1),0;
01035
01036 _Hest.block<3,1>(9,3) = a;
01037 _Hest.block<3,1>(12,3) = b;
01038 _Hest.block<3,1>(15,3) = c;
01039 _Hest.block<3,1>(9,4) = b;
01040 _Hest.block<3,1>(12,4) = d;
01041 _Hest.block<3,1>(15,4) = e;
01042 _Hest.block<3,1>(9,5) = c;
01043 _Hest.block<3,1>(12,5) = e;
01044 _Hest.block<3,1>(15,5) = f;
01045
01046
01047 myBlock<<
01048 0, -C1(0,1), -C1(0,2),
01049 -C1(0,1), 2*C1(2,2) - 2*C1(1,1), -4*C1(1,2),
01050 -C1(0,2), -4*C1(1,2), 2*C1(1,1) - 2*C1(2,2);
01051 _ZHest.block<3,3>(9,9) = myBlock;
01052
01053 myBlock<<
01054 0, C1(0,0) - C1(2,2), C1(1,2),
01055 C1(0,0) - C1(2,2), 2*C1(0,1), 2*C1(0,2),
01056 C1(1,2), 2*C1(0,2), -2*C1(0,1);
01057 _ZHest.block<3,3>(9,12) = myBlock;
01058
01059 myBlock<<
01060 0, C1(1,2), C1(0,0) - C1(1,1),
01061 C1(1,2), -2*C1(0,2), 2*C1(0,1),
01062 C1(0,0) - C1(1,1), 2*C1(0,1), 2*C1(0,2);
01063 _ZHest.block<3,3>(9,15) = myBlock;
01064
01065 myBlock<<
01066 2*C1(2,2) - 2*C1(0,0), -C1(0,1), -4*C1(0,2),
01067 -C1(0,1), 0, -C1(1,2),
01068 -4*C1(0,2), -C1(1,2), 2*C1(0,0) - 2*C1(2,2);
01069 _ZHest.block<3,3>(12,12) = myBlock;
01070
01071 myBlock<<
01072 -2*C1(1,2), C1(0,2), 2*C1(0,1),
01073 C1(0,2), 0, C1(1,1) - C1(0,0),
01074 2*C1(0,1), C1(1,1) - C1(0,0), 2*C1(1,2);
01075 _ZHest.block<3,3>(12,15) = myBlock;
01076
01077 myBlock<<
01078 2*C1(1,1) - 2*C1(0,0), -4*C1(0,1), -C1(0,2),
01079 -4*C1(0,1), 2*C1(0,0) - 2*C1(1,1), -C1(1,2),
01080 -C1(0,2), -C1(1,2), 0;
01081 _ZHest.block<3,3>(15,15)= myBlock;
01082
01083 _ZHest.block<3,3>(12,9) = _ZHest.block<3,3>(9,12);
01084 _ZHest.block<3,3>(15,9) = _ZHest.block<3,3>(9,15);
01085 _ZHest.block<3,3>(15,12)= _ZHest.block<3,3>(12,15);
01086 }
01087 }
01088
01089 template <typename PointSource, typename PointTarget>
01090 void NDTMatcherD2D<PointSource,PointTarget>::computeDerivatives(Eigen::Vector3d &x, Eigen::Matrix3d C1, bool computeHessian)
01091 {
01092
01093 Jest(0,4) = x(2);
01094 Jest(0,5) = -x(1);
01095 Jest(1,3) = -x(2);
01096 Jest(1,5) = x(0);
01097 Jest(2,3) = x(1);
01098 Jest(2,4) = -x(0);
01099
01100 Eigen::Matrix3d myBlock;
01101
01102 myBlock<<
01103 0, -C1(0,2), C1(0,1),
01104 -C1(0,2), -2*C1(1,2), -C1(2,2) + C1(1,1),
01105 C1(0,1), -C1(2,2) + C1(1,1), 2*C1(1,2);
01106 Zest.block<3,3>(0,9) = myBlock;
01107 myBlock<<
01108 2*C1(0,2), C1(1,2), -C1(0,0) + C1(2,2),
01109 C1(1,2), 0, -C1(0,1),
01110 -C1(0,0) + C1(2,2), -C1(0,1), -2*C1(0,2);
01111 Zest.block<3,3>(0,12) = myBlock;
01112 myBlock<<
01113 -2*C1(0,1), -C1(1,1) + C1(0,0), -C1(1,2),
01114 -C1(1,1) + C1(0,0), 2*C1(0,1), C1(0,2),
01115 -C1(1,2), C1(0,2), 0;
01116 Zest.block<3,3>(0,15) = myBlock;
01117
01118 if(computeHessian)
01119 {
01120 Eigen::Vector3d a,b,c,d,e,f;
01121 a<<0,-x(1),-x(2);
01122 b<<0,x(0),0;
01123 c<<0,0,x(0);
01124 d<<-x(0),0,-x(2);
01125 e<<0,0,x(1);
01126 f<<-x(0),-x(1),0;
01127
01128 Hest.block<3,1>(9,3) = a;
01129 Hest.block<3,1>(12,3) = b;
01130 Hest.block<3,1>(15,3) = c;
01131 Hest.block<3,1>(9,4) = b;
01132 Hest.block<3,1>(12,4) = d;
01133 Hest.block<3,1>(15,4) = e;
01134 Hest.block<3,1>(9,5) = c;
01135 Hest.block<3,1>(12,5) = e;
01136 Hest.block<3,1>(15,5) = f;
01137
01138
01139 myBlock<<
01140 0, -C1(0,1), -C1(0,2),
01141 -C1(0,1), 2*C1(2,2) - 2*C1(1,1), -4*C1(1,2),
01142 -C1(0,2), -4*C1(1,2), 2*C1(1,1) - 2*C1(2,2);
01143 ZHest.block<3,3>(9,9) = myBlock;
01144
01145 myBlock<<
01146 0, C1(0,0) - C1(2,2), C1(1,2),
01147 C1(0,0) - C1(2,2), 2*C1(0,1), 2*C1(0,2),
01148 C1(1,2), 2*C1(0,2), -2*C1(0,1);
01149 ZHest.block<3,3>(9,12) = myBlock;
01150
01151 myBlock<<
01152 0, C1(1,2), C1(0,0) - C1(1,1),
01153 C1(1,2), -2*C1(0,2), 2*C1(0,1),
01154 C1(0,0) - C1(1,1), 2*C1(0,1), 2*C1(0,2);
01155 ZHest.block<3,3>(9,15) = myBlock;
01156
01157 myBlock<<
01158 2*C1(2,2) - 2*C1(0,0), -C1(0,1), -4*C1(0,2),
01159 -C1(0,1), 0, -C1(1,2),
01160 -4*C1(0,2), -C1(1,2), 2*C1(0,0) - 2*C1(2,2);
01161 ZHest.block<3,3>(12,12) = myBlock;
01162
01163 myBlock<<
01164 -2*C1(1,2), C1(0,2), 2*C1(0,1),
01165 C1(0,2), 0, C1(1,1) - C1(0,0),
01166 2*C1(0,1), C1(1,1) - C1(0,0), 2*C1(1,2);
01167 ZHest.block<3,3>(12,15) = myBlock;
01168
01169 myBlock<<
01170 2*C1(1,1) - 2*C1(0,0), -4*C1(0,1), -C1(0,2),
01171 -4*C1(0,1), 2*C1(0,0) - 2*C1(1,1), -C1(1,2),
01172 -C1(0,2), -C1(1,2), 0;
01173 ZHest.block<3,3>(15,15)= myBlock;
01174
01175 ZHest.block<3,3>(12,9) = ZHest.block<3,3>(9,12);
01176 ZHest.block<3,3>(15,9) = ZHest.block<3,3>(9,15);
01177 ZHest.block<3,3>(15,12)= ZHest.block<3,3>(12,15);
01178 }
01179 }
01180
01181 template <typename PointSource, typename PointTarget>
01182 double NDTMatcherD2D<PointSource,PointTarget>::scoreNDT(std::vector<NDTCell<PointSource>*> &sourceNDT, NDTMap<PointTarget> &targetNDT)
01183 {
01184 NUMBER_OF_ACTIVE_CELLS = 0;
01185 NDTCell<PointTarget> *cell;
01186 Eigen::Vector3d transformed, eps;
01187 Eigen::Vector3d meanMoving, meanFixed;
01188 Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
01189
01190 bool exists = false;
01191 double det = 0;
01192 double score_here = 0;
01193
01194 PointTarget point;
01195 for(unsigned int i=0; i<sourceNDT.size(); i++)
01196 {
01197 meanMoving = sourceNDT[i]->getMean();
01198 CMoving= sourceNDT[i]->getCov();
01199 point.x = meanMoving(0);
01200 point.y = meanMoving(1);
01201 point.z = meanMoving(2);
01202 std::vector<NDTCell<PointSource>*> cells = targetNDT.getCellsForPoint(point,2);
01203 for(unsigned int j=0; j<cells.size(); j++)
01204 {
01205 cell = cells[j];
01206 if(cell == NULL)
01207 {
01208 continue;
01209 }
01210 if(cell->hasGaussian_)
01211 {
01212 transformed = meanMoving - cell->getMean();
01213 CFixed = cell->getCov();
01214 CSum = (CFixed+CMoving);
01215 CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
01216 if(!exists)
01217 {
01218 continue;
01219 }
01220 double l = (transformed).dot(Cinv*(transformed));
01221 if(l*0 != 0)
01222 {
01223 continue;
01224 }
01225
01226 double sh = -lfd1*(exp(-lfd2*l/2));
01227 NUMBER_OF_ACTIVE_CELLS++;
01228 score_here += sh;
01229 cell = NULL;
01230 }
01231 else
01232 {
01233
01234
01235 }
01236 }
01237 }
01238
01239 return score_here;
01240 }
01241
01242 template <typename PointSource, typename PointTarget>
01243 double NDTMatcherD2D<PointSource,PointTarget>::scoreNDT_OM(NDTMap<PointSource> &sourceNDT, NDTMap<PointTarget> &targetNDT)
01244 {
01245 NUMBER_OF_ACTIVE_CELLS = 0;
01246
01247 Eigen::Vector3d transformed;
01248 Eigen::Vector3d meanMoving, meanFixed;
01249 Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
01250 bool exists = false;
01251 double det = 0;
01252 double score_here = 0;
01253 double importance_free = 0.1;
01254
01255 PointTarget point;
01256 LazyGrid<PointSource>* lz = dynamic_cast<LazyGrid<PointSource>*> (sourceNDT.getMyIndex());
01257 if(lz==NULL) return INT_MAX;
01258 typename std::vector<Cell<PointSource>*>::iterator it = lz->begin();
01259
01260
01261
01262
01263 while(it!=lz->end()) {
01264 if(*it == NULL) {
01265 it++;
01266 continue;
01267 }
01268 NDTCell<PointSource>* source = dynamic_cast<NDTCell<PointSource>*>(*it);
01269 if(source==NULL) {
01270 it++;
01271 continue;
01272 }
01273 source->consistency_score = 0;
01274 NDTCell<PointSource>* cell=NULL;
01275 point = source->getCenter();
01276
01277 std::vector<NDTCell<PointSource>*> all_cells = targetNDT.getCellsForPoint(point,2,false);
01278
01279 for(unsigned int j=0; j<all_cells.size(); j++) {
01280 cell = all_cells[j];
01281 if(cell == NULL) {
01282 it++;
01283 continue;
01284 }
01285 double o1,o2;
01286 o1 = source->getOccupancyRescaled();
01287 o2 = cell->getOccupancyRescaled();
01288
01289
01290 if(source->hasGaussian_) {
01291 meanMoving = source->getMean();
01292 CMoving= source->getCov();
01293
01294
01295 if(cell->hasGaussian_) {
01296
01297 transformed = meanMoving - cell->getMean();
01298 CFixed = cell->getCov();
01299 CSum = (CFixed+CMoving);
01300 CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
01301 if(!exists)
01302 {
01303 it++;
01304 continue;
01305 }
01306 double l = (transformed).dot(Cinv*(transformed));
01307 if(l*0 != 0) {
01308 it++;
01309 continue;
01310 }
01311 double sh = -lfd1*(exp(-lfd2*l/2));
01312
01313 score_here += o1*o2*sh;
01314
01315
01316 source->consistency_score += -o1*o2*sh;
01317 }
01318 }
01319
01320 PointTarget cen1 = cell->getCenter();
01321 PointSource cen2 = source->getCenter();
01322 double cell_dist = std::sqrt(pow(cen1.x-cen2.x,2)+pow(cen1.y-cen2.y,2)+pow(cen1.z-cen2.z,2));
01323 double lambda = 1 - cell_dist/(cell->getDiagonal());
01324 lambda = lambda < 0 ? 0 : importance_free*lambda;
01325
01326 score_here += -lambda*((1-o1)*(1-o2) - o1*(1-o2) -o2*(1-o1));
01327
01328
01329
01330 double ll = cell->isInside(cen2) ? 1 : 0;
01331
01332 source->consistency_score += ll*((1-o1)*(1-o2) - o1*(1-o2) -o2*(1-o1));
01333 }
01334 it++;
01335 }
01336
01337
01338
01339
01340
01341
01342
01343
01344
01345
01346 return score_here;
01347 }
01348
01355 template <typename PointSource, typename PointTarget>
01356 double NDTMatcherD2D<PointSource,PointTarget>::scoreNDTPositive(std::vector<NDTCell<PointSource>*> &sourceNDT, NDTMap<PointTarget> &targetNDT,
01357 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T)
01358 {
01359
01360 NUMBER_OF_ACTIVE_CELLS = 0;
01361 double score_here = 0;
01362 double det = 0;
01363 bool exists = false;
01364 NDTCell<PointTarget> *cell;
01365 Eigen::Matrix3d covCombined, icov, R;
01366 Eigen::Vector3d meanFixed;
01367 Eigen::Vector3d meanMoving;
01368 PointTarget point;
01369
01370 R = T.rotation();
01371 for(unsigned int i=0; i<sourceNDT.size(); i++)
01372 {
01373 meanMoving = T*sourceNDT[i]->getMean();
01374 point.x = meanMoving(0);
01375 point.y = meanMoving(1);
01376 point.z = meanMoving(2);
01377
01378 if(!targetNDT.getCellForPoint(point,cell))
01379 {
01380 score_here += 0.1;
01381 continue;
01382 }
01383
01384 if(cell == NULL)
01385 {
01386 score_here += 0.1;
01387 continue;
01388 }
01389 if(cell->hasGaussian_)
01390 {
01391 meanFixed = cell->getMean();
01392 covCombined = cell->getCov() + R.transpose()*sourceNDT[i]->getCov()*R;
01393 covCombined.computeInverseAndDetWithCheck(icov,det,exists);
01394 if(!exists)
01395 {
01396 score_here+=0.1;
01397 continue;
01398 }
01399 double l = (meanMoving-meanFixed).dot(icov*(meanMoving-meanFixed));
01400 if(l*0 != 0)
01401 {
01402 score_here+=0.1;
01403 continue;
01404 }
01405 if(l > 120)
01406 {
01407 score_here+=0.1;
01408 continue;
01409 }
01410
01411 double sh = lfd1*(exp(-lfd2*l/2));
01412
01413 if(fabsf(sh) > 1e-10)
01414 {
01415 NUMBER_OF_ACTIVE_CELLS++;
01416 }
01417 score_here += (1.0 - sh);
01418
01419 }
01420 else
01421 {
01422 score_here+=0.1;
01423 }
01424
01425 }
01426 return score_here;
01427 }
01428
01432
01433
01434
01435 #define USE_OMP
01436
01437 template <typename PointSource, typename PointTarget>
01438
01439 double NDTMatcherD2D<PointSource,PointTarget>::derivativesNDT(
01440 const std::vector<NDTCell<PointSource>*> &sourceNDT,
01441 const NDTMap<PointTarget> &targetNDT,
01442 Eigen::MatrixXd &score_gradient,
01443 Eigen::MatrixXd &Hessian,
01444 bool computeHessian
01445 )
01446 {
01447
01448
01449 double score_here = 0;
01450 int n_dimensions = score_gradient.rows();
01451
01452
01453 NUMBER_OF_ACTIVE_CELLS = 0;
01454 score_gradient.setZero();
01455 Hessian.setZero();
01456
01457 #ifdef USE_OMP
01458 Eigen::MatrixXd score_gradient_omp;
01459 Eigen::MatrixXd score_here_omp;
01460 Eigen::MatrixXd Hessian_omp;
01461
01462 #define n_threads 2
01463
01464
01465 score_gradient_omp.resize(n_dimensions,n_threads);
01466 score_here_omp.resize(1,n_threads);
01467 Hessian_omp.resize(n_dimensions,n_dimensions*n_threads);
01468
01469 score_gradient_omp.setZero();
01470 score_here_omp.setZero();
01471 Hessian_omp.setZero();
01472
01473
01474 #pragma omp parallel num_threads(n_threads)
01475 {
01476 #pragma omp for
01477 for(unsigned int i=0; i<sourceNDT.size(); i++)
01478 {
01479 if(sourceNDT[i] == NULL) continue;
01480 if(!sourceNDT[i]->hasGaussian_) continue;
01481 PointTarget point;
01482 Eigen::Vector3d transformed;
01483 Eigen::Vector3d meanMoving, meanFixed;
01484 Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
01485 Eigen::MatrixXd score_gradient_omp_loc(n_dimensions,1);
01486 Eigen::MatrixXd Hessian_omp_loc(n_dimensions,n_dimensions);
01487 Eigen::Matrix<double,3,6> _Jest;
01488 Eigen::Matrix<double,18,6> _Hest;
01489 Eigen::Matrix<double,3,18> _Zest;
01490 Eigen::Matrix<double,18,18> _ZHest;
01491 double score_here_loc=0;
01492 int thread_id = omp_get_thread_num();
01493 NDTCell<PointTarget> *cell;
01494 bool exists = false;
01495 double det = 0;
01496
01497
01498 score_gradient_omp_loc.setZero();
01499 Hessian_omp_loc.setZero();
01500 _Jest.setZero();
01501 _Jest.block<3,3>(0,0).setIdentity();
01502 _Hest.setZero();
01503 _Zest.setZero();
01504 _ZHest.setZero();
01505
01506 meanMoving = sourceNDT[i]->getMean();
01507 CMoving= sourceNDT[i]->getCov();
01508 computeDerivativesLocal(meanMoving, CMoving, _Jest, _Hest, _Zest, _ZHest, computeHessian);
01509
01510 point.x = meanMoving(0);
01511 point.y = meanMoving(1);
01512 point.z = meanMoving(2);
01513 std::vector<NDTCell<PointSource>*> cells = targetNDT.getCellsForPoint(point,2);
01514 for(unsigned int j=0; j<cells.size(); j++)
01515 {
01516 cell = cells[j];
01517 if(cell == NULL)
01518 {
01519 continue;
01520 }
01521 if(cell->hasGaussian_)
01522 {
01523 transformed = meanMoving - cell->getMean();
01524 CFixed = cell->getCov();
01525 CSum = (CFixed+CMoving);
01526 CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
01527 if(!exists)
01528 {
01529 continue;
01530 }
01531 double l = (transformed).dot(Cinv*(transformed));
01532 if(l*0 != 0)
01533 {
01534 continue;
01535 }
01536
01537 double sh = -lfd1*(exp(-lfd2*l/2));
01538 if(!update_gradient_hessian_local(score_gradient_omp_loc,Hessian_omp_loc,transformed, Cinv, sh,
01539 _Jest, _Hest, _Zest, _ZHest, computeHessian))
01540 {
01541 continue;
01542 }
01543 score_here_loc += sh;
01544 cell = NULL;
01545 }
01546 }
01547
01548 score_gradient_omp.col(thread_id) += score_gradient_omp_loc;
01549 Hessian_omp.block(0,n_dimensions*thread_id,n_dimensions,n_dimensions) += Hessian_omp_loc;
01550 score_here_omp(0,thread_id) += score_here_loc;
01551
01552 }
01553 }
01554
01555
01556
01557 score_gradient = score_gradient_omp.rowwise().sum();
01558 score_here = score_here_omp.sum();
01559 if(computeHessian)
01560 {
01561
01562 for(int i=0; i<n_threads; ++i)
01563 {
01564 Hessian += Hessian_omp.block(0,n_dimensions*i,n_dimensions,n_dimensions);
01565 }
01566 }
01567 #else
01568 PointTarget point;
01569 Eigen::Vector3d transformed;
01570 Eigen::Vector3d meanMoving, meanFixed;
01571 Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
01572 NDTCell<PointTarget> *cell;
01573 bool exists = false;
01574 double det = 0;
01575 for(unsigned int i=0; i<sourceNDT.size(); i++)
01576 {
01577 meanMoving = sourceNDT[i]->getMean();
01578 CMoving= sourceNDT[i]->getCov();
01579 computeDerivatives(meanMoving, CMoving, computeHessian);
01580
01581 point.x = meanMoving(0);
01582 point.y = meanMoving(1);
01583 point.z = meanMoving(2);
01584 std::vector<NDTCell<PointSource>*> cells = targetNDT.getCellsForPoint(point,2);
01585 for(int j=0; j<cells.size(); j++)
01586 {
01587 cell = cells[j];
01588 if(cell == NULL)
01589 {
01590 continue;
01591 }
01592 if(cell->hasGaussian_)
01593 {
01594 transformed = meanMoving - cell->getMean();
01595 CFixed = cell->getCov();
01596 CSum = (CFixed+CMoving);
01597 CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
01598 if(!exists)
01599 {
01600
01601 continue;
01602 }
01603 double l = (transformed).dot(Cinv*(transformed));
01604 if(l*0 != 0)
01605 {
01606
01607 continue;
01608 }
01609
01610 double sh = -lfd1*(exp(-lfd2*l/2));
01611
01612
01613
01614
01615
01616 if(!update_gradient_hessian(score_gradient,Hessian,transformed, Cinv, sh, computeHessian))
01617 {
01618
01619 continue;
01620 }
01621
01622 score_here += sh;
01623
01624 cell = NULL;
01625 }
01626 }
01627 }
01628 #endif
01629
01630
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01642
01643
01644
01645
01646
01647
01648
01649
01650
01651
01652
01653
01654
01655
01656
01657
01658
01659
01660
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674 return score_here;
01675 }
01676
01677
01678
01679 template <typename PointSource, typename PointTarget>
01680 double NDTMatcherD2D<PointSource,PointTarget>::lineSearchMT(
01681 Eigen::Matrix<double,6,1> &increment,
01682 std::vector<NDTCell<PointSource>*> &sourceNDT,
01683 NDTMap<PointTarget> &targetNDT
01684 )
01685 {
01686
01687
01688 double stp = 1.0;
01689 double recoverystep = 0.1;
01690 double dginit = 0.0;
01691 double ftol = 0.11111;
01692 double gtol = 0.99999;
01693 double stpmax = 4.0;
01694 double stpmin = 0.001;
01695 int maxfev = 40;
01696 double xtol = 0.01;
01697
01698
01699 std::vector<NDTCell<PointSource>*> sourceNDTHere;
01700 double score_init = 0.0;
01701
01702 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> ps;
01703 ps.setIdentity();
01704
01705 Eigen::Matrix<double,6,1> scg_here;
01706 Eigen::MatrixXd pincr(6,1), score_gradient_here(6,1);
01707 Eigen::MatrixXd pseudoH(6,6);
01708 Eigen::Vector3d eulerAngles;
01710
01711 int info = 0;
01712 int infoc = 1;
01713
01714
01715
01716
01717
01718
01719
01720
01721
01722
01723
01724
01725
01726
01727 score_gradient_here.setZero();
01728 score_init = derivativesNDT(sourceNDT,targetNDT,score_gradient_here,pseudoH,false);
01729 scg_here = score_gradient_here;
01730 dginit = increment.dot(scg_here);
01731
01732
01733 if (dginit >= 0.0)
01734 {
01735 std::cout << "MoreThuente::cvsrch - wrong direction (dginit = " << dginit << ")" << std::endl;
01736
01737
01738
01739 increment = -increment;
01740 dginit = -dginit;
01741
01742 if (dginit >= 0.0)
01743 {
01744 for(unsigned int i=0; i<sourceNDTHere.size(); i++)
01745 {
01746 if(sourceNDTHere[i]!=NULL)
01747 delete sourceNDTHere[i];
01748 }
01749 return recoverystep;
01750 }
01751 }
01752 else
01753 {
01754
01755 }
01756
01757
01758
01759 bool brackt = false;
01760 bool stage1 = true;
01761 int nfev = 0;
01762 double dgtest = ftol * dginit;
01763 double width = stpmax - stpmin;
01764 double width1 = 2 * width;
01765
01766
01767
01768 double finit = 0.0;
01769 finit = score_init;
01770
01771
01772
01773
01774
01775
01776
01777
01778 double stx = 0.0;
01779 double fx = finit;
01780 double dgx = dginit;
01781 double sty = 0.0;
01782 double fy = finit;
01783 double dgy = dginit;
01784
01785
01786
01787
01788
01789
01790
01791
01792 double stmin, stmax;
01793 double fm, fxm, fym, dgm, dgxm, dgym;
01794
01795 while (1)
01796 {
01797
01798
01799 if (brackt)
01800 {
01801 stmin = MoreThuente::min(stx, sty);
01802 stmax = MoreThuente::max(stx, sty);
01803 }
01804 else
01805 {
01806 stmin = stx;
01807 stmax = stp + 4 * (stp - stx);
01808 }
01809
01810
01811 stp = MoreThuente::max(stp, stpmin);
01812 stp = MoreThuente::min(stp, stpmax);
01813
01814
01815
01816
01817 if ((brackt && ((stp <= stmin) || (stp >= stmax))) ||
01818 (nfev >= maxfev - 1) || (infoc == 0) ||
01819 (brackt && (stmax - stmin <= xtol * stmax)))
01820 {
01821 stp = stx;
01822 }
01823
01824
01825
01827
01828 pincr = stp*increment;
01829
01830 ps = Eigen::Translation<double,3>(pincr(0),pincr(1),pincr(2))*
01831 Eigen::AngleAxisd(pincr(3),Eigen::Vector3d::UnitX())*
01832 Eigen::AngleAxisd(pincr(4),Eigen::Vector3d::UnitY())*
01833 Eigen::AngleAxisd(pincr(5),Eigen::Vector3d::UnitZ());
01834
01835 for(unsigned int i=0; i<sourceNDTHere.size(); i++)
01836 {
01837 if(sourceNDTHere[i]!=NULL)
01838 delete sourceNDTHere[i];
01839 }
01840 sourceNDTHere.clear();
01841 for(unsigned int i=0; i<sourceNDT.size(); i++)
01842 {
01843 NDTCell<PointSource> *cell = sourceNDT[i];
01844 if(cell!=NULL)
01845 {
01846 Eigen::Vector3d mean = cell->getMean();
01847 Eigen::Matrix3d cov = cell->getCov();
01848 mean = ps*mean;
01849 cov = ps.rotation()*cov*ps.rotation().transpose();
01850 NDTCell<PointSource>* nd = (NDTCell<PointSource>*)cell->copy();
01851 nd->setMean(mean);
01852 nd->setCov(cov);
01853 sourceNDTHere.push_back(nd);
01854 }
01855 }
01856
01857 double f = 0.0;
01858 score_gradient_here.setZero();
01859
01860
01861
01862
01863
01864
01865
01866
01867 f = derivativesNDT(sourceNDTHere,targetNDT,score_gradient_here,pseudoH,false);
01868
01869
01870
01871
01872
01873
01874 double dg = 0.0;
01875 scg_here = score_gradient_here;
01876 dg = increment.dot(scg_here);
01877
01878
01879
01880
01881 nfev ++;
01882
01884
01885
01886
01887 double ftest1 = finit + stp * dgtest;
01888
01889
01890
01891
01892 if ((brackt && ((stp <= stmin) || (stp >= stmax))) || (infoc == 0))
01893 info = 6;
01894
01895 if ((stp == stpmax) && (f <= ftest1) && (dg <= dgtest))
01896 info = 5;
01897
01898 if ((stp == stpmin) && ((f > ftest1) || (dg >= dgtest)))
01899 info = 4;
01900
01901 if (nfev >= maxfev)
01902 info = 3;
01903
01904 if (brackt && (stmax-stmin <= xtol*stmax))
01905 info = 2;
01906
01907
01908 bool sufficientDecreaseTest = false;
01909 sufficientDecreaseTest = (f <= ftest1);
01910
01911
01912
01913
01914 if ((sufficientDecreaseTest) && (fabs(dg) <= gtol*(-dginit)))
01915 info = 1;
01916
01917 if (info != 0)
01918 {
01919 if (info != 1)
01920 {
01921
01922
01923
01924
01925 stp = recoverystep;
01926
01927
01928
01929
01930
01931 }
01932 else
01933 {
01934
01935 }
01936
01937
01938
01939
01940
01941 for(unsigned int i=0; i<sourceNDTHere.size(); i++)
01942 {
01943 if(sourceNDTHere[i]!=NULL)
01944 delete sourceNDTHere[i];
01945 }
01946
01947 return stp;
01948
01949 }
01950
01951
01952
01953
01954
01955
01956
01957 if (stage1 && (f <= ftest1) && (dg >= MoreThuente::min(ftol, gtol) * dginit))
01958 {
01959 stage1 = false;
01960 }
01961
01962
01963
01964
01965
01966
01967
01968 if (stage1 && (f <= fx) && (f > ftest1))
01969 {
01970
01971
01972
01973 fm = f - stp * dgtest;
01974 fxm = fx - stx * dgtest;
01975 fym = fy - sty * dgtest;
01976 dgm = dg - dgtest;
01977 dgxm = dgx - dgtest;
01978 dgym = dgy - dgtest;
01979
01980
01981
01982
01983
01984 infoc = MoreThuente::cstep(stx,fxm,dgxm,sty,fym,dgym,stp,fm,dgm,
01985 brackt,stmin,stmax);
01986
01987
01988
01989 fx = fxm + stx*dgtest;
01990 fy = fym + sty*dgtest;
01991 dgx = dgxm + dgtest;
01992 dgy = dgym + dgtest;
01993
01994 }
01995
01996 else
01997 {
01998
01999
02000
02001
02002
02003 infoc = MoreThuente::cstep(stx,fx,dgx,sty,fy,dgy,stp,f,dg,
02004 brackt,stmin,stmax);
02005
02006 }
02007
02008
02009
02010
02011 if (brackt)
02012 {
02013 if (fabs(sty - stx) >= 0.66 * width1)
02014 stp = stx + 0.5 * (sty - stx);
02015 width1 = width;
02016 width = fabs(sty-stx);
02017 }
02018
02019 }
02020
02021 }
02022
02023 template <typename PointSource, typename PointTarget>
02024 int NDTMatcherD2D<PointSource,PointTarget>::MoreThuente::cstep(double& stx, double& fx, double& dx,
02025 double& sty, double& fy, double& dy,
02026 double& stp, double& fp, double& dp,
02027 bool& brackt, double stmin, double stmax)
02028 {
02029 int info = 0;
02030
02031
02032
02033 if ((brackt && ((stp <= MoreThuente::min(stx, sty)) || (stp >= MoreThuente::max(stx, sty)))) ||
02034 (dx * (stp - stx) >= 0.0) || (stmax < stmin))
02035 return info;
02036
02037
02038
02039 double sgnd = dp * (dx / fabs(dx));
02040
02041
02042
02043
02044
02045
02046 bool bound;
02047 double theta;
02048 double s;
02049 double gamma;
02050 double p,q,r;
02051 double stpc, stpq, stpf;
02052
02053 if (fp > fx)
02054 {
02055 info = 1;
02056 bound = 1;
02057 theta = 3 * (fx - fp) / (stp - stx) + dx + dp;
02058
02059
02060
02061 s = MoreThuente::absmax(theta, dx, dp);
02062 gamma = s * sqrt(((theta / s) * (theta / s)) - (dx / s) * (dp / s));
02063 if (stp < stx)
02064 gamma = -gamma;
02065
02066 p = (gamma - dx) + theta;
02067 q = ((gamma - dx) + gamma) + dp;
02068 r = p / q;
02069 stpc = stx + r * (stp - stx);
02070 stpq = stx + ((dx / ((fx - fp) / (stp - stx) + dx)) / 2) * (stp - stx);
02071 if (fabs(stpc - stx) < fabs(stpq - stx))
02072 stpf = stpc;
02073 else
02074 stpf = stpc + (stpq - stpc) / 2;
02075
02076 brackt = true;
02077 }
02078
02079
02080
02081
02082
02083
02084 else if (sgnd < 0.0)
02085 {
02086 info = 2;
02087 bound = false;
02088 theta = 3 * (fx - fp) / (stp - stx) + dx + dp;
02089 s = MoreThuente::absmax(theta,dx,dp);
02090 gamma = s * sqrt(((theta/s) * (theta/s)) - (dx / s) * (dp / s));
02091 if (stp > stx)
02092 gamma = -gamma;
02093 p = (gamma - dp) + theta;
02094 q = ((gamma - dp) + gamma) + dx;
02095 r = p / q;
02096 stpc = stp + r * (stx - stp);
02097 stpq = stp + (dp / (dp - dx)) * (stx - stp);
02098 if (fabs(stpc - stp) > fabs(stpq - stp))
02099 stpf = stpc;
02100 else
02101 stpf = stpq;
02102 brackt = true;
02103 }
02104
02105
02106
02107
02108
02109
02110
02111
02112
02113
02114 else if (fabs(dp) < fabs(dx))
02115 {
02116 info = 3;
02117 bound = true;
02118 theta = 3 * (fx - fp) / (stp - stx) + dx + dp;
02119 s = MoreThuente::absmax(theta, dx, dp);
02120
02121
02122
02123
02124 gamma = s * sqrt(max(0,(theta / s) * (theta / s) - (dx / s) * (dp / s)));
02125 if (stp > stx)
02126 gamma = -gamma;
02127
02128 p = (gamma - dp) + theta;
02129 q = (gamma + (dx - dp)) + gamma;
02130 r = p / q;
02131 if ((r < 0.0) && (gamma != 0.0))
02132 stpc = stp + r * (stx - stp);
02133 else if (stp > stx)
02134 stpc = stmax;
02135 else
02136 stpc = stmin;
02137
02138 stpq = stp + (dp/ (dp - dx)) * (stx - stp);
02139 if (brackt)
02140 {
02141 if (fabs(stp - stpc) < fabs(stp - stpq))
02142 stpf = stpc;
02143 else
02144 stpf = stpq;
02145 }
02146 else
02147 {
02148 if (fabs(stp - stpc) > fabs(stp - stpq))
02149 stpf = stpc;
02150 else
02151 stpf = stpq;
02152 }
02153 }
02154
02155
02156
02157
02158
02159
02160 else
02161 {
02162 info = 4;
02163 bound = false;
02164 if (brackt)
02165 {
02166 theta = 3 * (fp - fy) / (sty - stp) + dy + dp;
02167 s = MoreThuente::absmax(theta, dy, dp);
02168 gamma = s * sqrt(((theta/s)*(theta/s)) - (dy / s) * (dp / s));
02169 if (stp > sty)
02170 gamma = -gamma;
02171 p = (gamma - dp) + theta;
02172 q = ((gamma - dp) + gamma) + dy;
02173 r = p / q;
02174 stpc = stp + r * (sty - stp);
02175 stpf = stpc;
02176 }
02177 else if (stp > stx)
02178 stpf = stmax;
02179 else
02180 stpf = stmin;
02181 }
02182
02183
02184
02185
02186 if (fp > fx)
02187 {
02188 sty = stp;
02189 fy = fp;
02190 dy = dp;
02191 }
02192 else
02193 {
02194 if (sgnd < 0.0)
02195 {
02196 sty = stx;
02197 fy = fx;
02198 dy = dx;
02199 }
02200 stx = stp;
02201 fx = fp;
02202 dx = dp;
02203 }
02204
02205
02206
02207 stpf = MoreThuente::min(stmax, stpf);
02208 stpf = MoreThuente::max(stmin, stpf);
02209 stp = stpf;
02210 if (brackt && bound)
02211 {
02212 if (sty > stx)
02213 stp = min(stx + 0.66 * (sty - stx), stp);
02214 else
02215 stp = max(stx + 0.66 * (sty - stx), stp);
02216 }
02217
02218 return info;
02219
02220 }
02221
02222 template <typename PointSource, typename PointTarget>
02223 double NDTMatcherD2D<PointSource,PointTarget>::MoreThuente::min(double a, double b)
02224 {
02225 return (a < b ? a : b);
02226 }
02227
02228 template <typename PointSource, typename PointTarget>
02229 double NDTMatcherD2D<PointSource,PointTarget>::MoreThuente::max(double a, double b)
02230 {
02231 return (a > b ? a : b);
02232 }
02233
02234 template <typename PointSource, typename PointTarget>
02235 double NDTMatcherD2D<PointSource,PointTarget>::MoreThuente::absmax(double a, double b, double c)
02236 {
02237 a = fabs(a);
02238 b = fabs(b);
02239 c = fabs(c);
02240
02241 if (a > b)
02242 return (a > c) ? a : c;
02243 else
02244 return (b > c) ? b : c;
02245 }
02246
02247 template <typename PointSource, typename PointTarget>
02248 double NDTMatcherD2D<PointSource,PointTarget>::normalizeAngle(double a)
02249 {
02250
02251 return atan2(sin(a), cos(a));
02252
02253 }
02254
02255
02256
02257
02258
02259
02260
02261
02262
02263
02264
02265
02266
02267
02268
02269
02270
02271
02272
02273
02274
02275
02276
02277
02278
02279
02280
02281
02282
02283
02284
02285
02286
02287
02288
02289
02290
02291
02292
02293
02294
02295
02296
02297
02298
02299
02300
02301
02302
02303
02304
02305
02306
02307
02308
02309
02310
02311
02312
02313
02314
02315
02316
02317
02318
02319
02320
02321
02322
02323
02324
02325
02326
02327
02328
02329
02330
02331
02332
02333
02334
02335
02336
02337
02338
02339
02340
02341
02342
02343
02344
02345
02346
02347
02348
02349
02350
02351
02352
02353
02354
02355
02356
02357
02358
02359
02360
02361
02362
02363
02364
02365
02366
02367
02368
02369
02370
02371
02372
02373
02374
02375
02376
02377
02378
02379
02380
02381
02382
02383
02384
02385
02386
02387
02388
02389
02390
02391
02392
02393
02394
02395
02396
02397
02398
02399
02400
02401
02402
02403
02404
02405
02406
02407
02408
02409
02410
02411 template <typename PointSource, typename PointTarget>
02412 bool NDTMatcherD2D<PointSource,PointTarget>::covariance( NDTMap<PointTarget>& targetNDT,
02413 NDTMap<PointSource>& sourceNDT,
02414 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
02415 Eigen::MatrixXd &cov
02416 )
02417 {
02418
02419 double sigmaS = (0.03)*(0.03);
02420 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR;
02421 TR.setIdentity();
02422
02423 std::vector<NDTCell<PointSource>*> sourceNDTN = sourceNDT.pseudoTransformNDT(T);
02424 std::vector<NDTCell<PointTarget>*> targetNDTN = targetNDT.pseudoTransformNDT(T);
02425
02426 Eigen::MatrixXd scg(6,1);
02427 int NM = sourceNDTN.size() + targetNDTN.size();
02428
02429 Eigen::MatrixXd Jdpdz(NM,6);
02430
02431 NDTCell<PointTarget> *cell;
02432 Eigen::Vector3d transformed;
02433 Eigen::Vector3d meanMoving, meanFixed;
02434 Eigen::Matrix3d CMoving, CFixed, Cinv;
02435 bool exists = false;
02436 double det = 0;
02437 Eigen::Matrix<double,6,1> ones;
02438 ones<<1,1,1,1,1,1;
02439
02440 derivativesNDT(sourceNDTN,targetNDT,scg,cov,true);
02441
02442 Eigen::Matrix3d Q;
02443 Jdpdz.setZero();
02444 Q.setZero();
02445
02446 PointTarget point;
02447
02448 for(int i=0; i<sourceNDTN.size(); i++)
02449 {
02450 meanMoving = sourceNDTN[i]->getMean();
02451 point.x = meanMoving(0);
02452 point.y = meanMoving(1);
02453 point.z = meanMoving(2);
02454
02455 if(!targetNDT.getCellForPoint(point,cell))
02456 {
02457 continue;
02458 }
02459 if(cell == NULL)
02460 {
02461 continue;
02462 }
02463 if(cell->hasGaussian_)
02464 {
02465
02466 meanFixed = cell->getMean();
02467 transformed = meanMoving-meanFixed;
02468 CFixed = cell->getCov();
02469 CMoving= sourceNDTN[i]->getCov();
02470
02471 (CFixed+CMoving).computeInverseAndDetWithCheck(Cinv,det,exists);
02472 if(!exists) continue;
02473
02474
02475 double factor = (-transformed.dot(Cinv*transformed)/2);
02476
02477 if(factor < -120)
02478 {
02479 continue;
02480 }
02481 factor = exp(lfd2*factor)/2;
02482 if(factor > 1 || factor < 0 || factor*0 !=0)
02483 {
02484 continue;
02485 }
02486
02487 Q = -sigmaS*Cinv*Cinv;
02488
02489 Eigen::Matrix<double,6,1> G, xtQJ;
02490
02491 G.setZero();
02492 for(int q=3; q<6; q++)
02493 {
02494 G(q) = -transformed.transpose()*Q*Zest.block<3,3>(0,3*q)*Cinv*transformed;
02495 G(q) = G(q) -transformed.transpose()*Cinv*Zest.block<3,3>(0,3*q)*Q*transformed;
02496 }
02497
02498 xtQJ = transformed.transpose()*Q*Jest;
02499
02500 double f1 = (transformed.transpose()*Q*transformed);
02501 G = G + xtQJ + (-lfd2/2)*f1*ones;
02502 G = G*factor*lfd1*lfd2/2;
02503
02504 Jdpdz.row(i) = G.transpose();
02505
02506 for(int j=0; j<targetNDTN.size(); j++)
02507 {
02508 if(targetNDTN[j]->getMean() == meanFixed)
02509 {
02510
02511 Jdpdz.row(j+sourceNDTN.size()) = Jdpdz.row(j+sourceNDTN.size())+G.transpose();
02512 continue;
02513 }
02514 }
02515
02516 cell = NULL;
02517 }
02518 }
02519
02520
02521
02522
02523 Eigen::MatrixXd JK(6,6);
02524 JK = sigmaS*Jdpdz.transpose()*Jdpdz;
02525
02526
02527
02528
02529 cov = cov.inverse()*JK*cov.inverse();
02530
02531
02532
02533 for(unsigned int q=0; q<sourceNDTN.size(); q++)
02534 {
02535 delete sourceNDTN[q];
02536 }
02537 sourceNDTN.clear();
02538
02539 return true;
02540 }
02541 template <typename PointSource, typename PointTarget>
02542 bool NDTMatcherD2D<PointSource,PointTarget>::covariance( pcl::PointCloud<PointTarget>& target,
02543 pcl::PointCloud<PointSource>& source,
02544 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
02545 Eigen::MatrixXd &cov
02546 )
02547 {
02548
02549 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR;
02550 TR.setIdentity();
02551
02552 pcl::PointCloud<PointSource> sourceCloud = lslgeneric::transformPointCloud(T,source);
02553
02554 LazyGrid<PointSource> prototypeSource(resolutions.front());
02555 LazyGrid<PointTarget> prototypeTarget(resolutions.front());
02556
02557 NDTMap<PointTarget> targetNDT( &prototypeTarget );
02558 targetNDT.loadPointCloud( target );
02559 targetNDT.computeNDTCells();
02560
02561 NDTMap<PointSource> sourceNDT( &prototypeSource );
02562 sourceNDT.loadPointCloud( sourceCloud );
02563 sourceNDT.computeNDTCells();
02564
02565 this->covariance(targetNDT,sourceNDT,TR,cov);
02566
02567 return true;
02568 }
02569
02570
02571 }