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_2D<PointSource,PointTarget>::init(bool _isIrregularGrid,
00016 bool useDefaultGridResolutions, std::vector<double> _resolutions)
00017 {
00018 Jest.setZero();
00019 Jest.block<2,2>(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 = 50;
00041 DELTA_SCORE = 10e-3*current_resolution;
00042 step_control = true;
00043
00044 }
00045
00046 template <typename PointSource, typename PointTarget>
00047 bool NDTMatcherD2D_2D<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
00062 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Temp, Tinit;
00063 Tinit.setIdentity();
00064 if(useInitialGuess)
00065 {
00066 lslgeneric::transformPointCloudInPlace(T,sourceCloud);
00067 Tinit = T;
00068 }
00069
00070 T.setIdentity();
00071 bool ret = false;
00072
00073 #ifdef DO_DEBUG_PROC
00074 char fname[50];
00075 snprintf(fname,49,"initial_clouds.wrl");
00076 FILE *fout = fopen(fname,"w");
00077 fprintf(fout,"#VRML V2.0 utf8\n");
00078
00079 lslgeneric::writeToVRML(fout,target,Eigen::Vector3d(1,0,0));
00080 lslgeneric::writeToVRML(fout,source,Eigen::Vector3d(0,1,0));
00081 fclose(fout);
00082 #endif
00083
00084 if(isIrregularGrid)
00085 {
00086
00087 OctTree<PointTarget> pr1;
00088 NDTMap<PointTarget> targetNDT( &pr1 );
00089 targetNDT.loadPointCloud( target );
00090 targetNDT.computeNDTCells();
00091
00092 OctTree<PointSource> pr2;
00093 NDTMap<PointSource> sourceNDT( &pr2 );
00094 sourceNDT.loadPointCloud( source );
00095 sourceNDT.computeNDTCells();
00096
00097 ret = this->match( targetNDT, sourceNDT, T );
00098
00099 }
00100 else
00101 {
00102
00103
00104 for(int r_ctr = resolutions.size()-1; r_ctr >=0; r_ctr--)
00105 {
00106
00107 current_resolution = resolutions[r_ctr];
00108
00109 LazyGrid<PointSource> prototypeSource(current_resolution);
00110 LazyGrid<PointTarget> prototypeTarget(current_resolution);
00111
00112 gettimeofday(&tv_start,NULL);
00113 NDTMap<PointTarget> targetNDT( &prototypeTarget );
00114 targetNDT.loadPointCloud( target );
00115 targetNDT.computeNDTCells();
00116
00117 NDTMap<PointSource> sourceNDT( &prototypeSource );
00118 sourceNDT.loadPointCloud( sourceCloud );
00119 sourceNDT.computeNDTCells();
00120 gettimeofday(&tv_end,NULL);
00121
00122 time_load += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00123 Temp.setIdentity();
00124
00125 gettimeofday(&tv_start,NULL);
00126 ret = this->match( targetNDT, sourceNDT, Temp );
00127 lslgeneric::transformPointCloudInPlace(Temp,sourceCloud);
00128 gettimeofday(&tv_end,NULL);
00129
00130 time_match += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00131
00132
00133 T = Temp*T;
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 gettimeofday(&tv_end0,NULL);
00173 time_combined = (tv_end0.tv_sec-tv_start0.tv_sec)*1000.+(tv_end0.tv_usec-tv_start0.tv_usec)/1000.;
00174 std::cout<<"load: "<<time_load<<" match "<<time_match<<" combined "<<time_combined<<std::endl;
00175 if(useInitialGuess)
00176 {
00177 T = T*Tinit;
00178 }
00179 return ret;
00180 }
00181
00182 template <typename PointSource, typename PointTarget>
00183 bool NDTMatcherD2D_2D<PointSource,PointTarget>::match( NDTMap<PointTarget>& targetNDT,
00184 NDTMap<PointSource>& sourceNDT,
00185 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T ,
00186 bool useInitialGuess)
00187 {
00188
00189
00190 bool convergence = false;
00191 int itr_ctr = 0;
00192 double step_size = 1;
00193 Eigen::Matrix<double,3,1> pose_increment_v, scg;
00194
00195 Eigen::MatrixXd H(3,3), score_gradient_2d(3,1);
00196 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR, Tbest;
00197 Eigen::Vector3d transformed_vec, mean;
00198 bool ret = true;
00199 double score_best = INT_MAX;
00200 if(!useInitialGuess)
00201 {
00202 T.setIdentity();
00203 }
00204 Tbest = T;
00205
00206 std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00207 while(!convergence)
00208 {
00209 TR.setIdentity();
00210 H.setZero();
00211 score_gradient_2d.setZero();
00212
00213
00214 double score_here = derivativesNDT_2d(nextNDT,targetNDT,score_gradient_2d,H,true);
00215 scg = score_gradient_2d;
00216
00217 if(score_here < score_best)
00218 {
00219 Tbest = T;
00220 score_best = score_here;
00221
00222 }
00223
00224 if (score_gradient_2d.norm()<= 10e-2*DELTA_SCORE)
00225 {
00226 std::cout<<"\%gradient vanished\n";
00227 for(unsigned int i=0; i<nextNDT.size(); i++)
00228 {
00229 if(nextNDT[i]!=NULL)
00230 delete nextNDT[i];
00231 }
00232 if(score_here > score_best)
00233 {
00234 std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
00235 T = Tbest;
00236 }
00237 return true;
00238 }
00239
00240
00241
00242 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,3,3> > Sol (H);
00243 Eigen::Matrix<double,3,1> evals = Sol.eigenvalues().real();
00244 double minCoeff = evals.minCoeff();
00245 double maxCoeff = evals.maxCoeff();
00246 if(minCoeff < 10e-3 )
00247 {
00248 Eigen::Matrix<double,3,3> evecs = Sol.eigenvectors().real();
00249 double regularizer = score_gradient_2d.norm();
00250 regularizer = regularizer + minCoeff > 0 ? regularizer : 0.001*maxCoeff - minCoeff;
00251
00252 Eigen::Matrix<double,3,1> reg;
00253
00254 reg<<regularizer,regularizer,regularizer;
00255 evals += reg;
00256 Eigen::Matrix<double,3,3> Lam;
00257 Lam = evals.asDiagonal();
00258 H = evecs*Lam*(evecs.transpose());
00259
00260 }
00261
00262
00263
00264 pose_increment_v = -H.ldlt().solve(scg);
00265 double dginit = pose_increment_v.dot(scg);
00266 if(dginit > 0)
00267 {
00268 std::cout<<"\%can't decrease in this direction any more, done \n";
00269 for(unsigned int i=0; i<nextNDT.size(); i++)
00270 {
00271 if(nextNDT[i]!=NULL)
00272 delete nextNDT[i];
00273 }
00274 if(score_here > score_best)
00275 {
00276
00277 T = Tbest;
00278 }
00279 return true;
00280 }
00281 if(step_control) {
00282 step_size = lineSearch2D(pose_increment_v,nextNDT,targetNDT);
00283 } else {
00284 step_size = 1;
00285 }
00286 pose_increment_v = step_size*pose_increment_v;
00287 TR.setIdentity();
00288 TR = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),0)*
00289 Eigen::AngleAxis<double>(pose_increment_v(2),Eigen::Vector3d::UnitZ()) ;
00290 T = TR*T;
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300 for(unsigned int i=0; i<nextNDT.size(); i++)
00301 {
00302
00303 Eigen::Vector3d meanC = nextNDT[i]->getMean();
00304 Eigen::Matrix3d covC = nextNDT[i]->getCov();
00305 meanC = TR*meanC;
00306 covC = TR.rotation()*covC*TR.rotation().transpose();
00307 nextNDT[i]->setMean(meanC);
00308 nextNDT[i]->setCov(covC);
00309 }
00310
00311 if(itr_ctr>0)
00312 {
00313 convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
00314 }
00315 if(itr_ctr>ITR_MAX)
00316 {
00317 convergence = true;
00318 ret = false;
00319 }
00320 itr_ctr++;
00321 }
00322
00323
00324 score_gradient_2d.setZero();
00325
00326 double score_here = derivativesNDT_2d(nextNDT,targetNDT,score_gradient_2d,H,false);
00327 if(score_here > score_best)
00328 {
00329
00330 T = Tbest;
00331 }
00332 for(unsigned int i=0; i<nextNDT.size(); i++)
00333 {
00334 if(nextNDT[i]!=NULL)
00335 delete nextNDT[i];
00336 }
00337
00338 return ret;
00339 }
00340
00341
00342 template <typename PointSource, typename PointTarget>
00343 bool NDTMatcherD2D_2D<PointSource,PointTarget>::update_gradient_hessian_local_2d(
00344 Eigen::MatrixXd &score_gradient,
00345 Eigen::MatrixXd &Hessian,
00346 const Eigen::Vector3d & x,
00347 const Eigen::Matrix3d & B,
00348 const double &likelihood,
00349 const Eigen::Matrix<double,3,3> &_Jest,
00350 const Eigen::Matrix<double,9,3> &_Hest,
00351 const Eigen::Matrix<double,3,9> &_Zest,
00352 const Eigen::Matrix<double,9,9> &_ZHest,
00353 bool computeHessian)
00354 {
00355
00356
00357 Eigen::Matrix<double,3,1> _xtBJ, _xtBZBx, _Q;
00358
00359 Eigen::Matrix<double,3,3> _xtBZBJ, _xtBH, _xtBZBZBx, _xtBZhBx;
00360 Eigen::Matrix<double,1,3> _TMP1, _xtB;
00361
00362 _xtBJ.setZero();
00363 _xtBZBx.setZero();
00364 _Q.setZero();
00365 _xtBZBJ.setZero();
00366 _xtBH.setZero();
00367 _xtBZBZBx.setZero();
00368 _xtBZhBx.setZero();
00369 _TMP1.setZero();
00370 _xtB.setZero();
00371
00372 _xtB = x.transpose()*B;
00373 _xtBJ = _xtB*_Jest;
00374
00375
00376
00377 _TMP1 = _xtB*_Zest.block<3,3>(0,6)*B;
00378 _xtBZBx(2) = _TMP1*x;
00379 if(computeHessian)
00380 {
00381 _xtBZBJ.col(2) = (_TMP1*_Jest).transpose();
00382 for(unsigned int j=0; j<3; j++)
00383 {
00384 _xtBH(2,j) = _xtB*_Hest.block<3,1>(6,j);
00385 _xtBZBZBx(2,j) = _TMP1*_Zest.block<3,3>(0,3*j)*B*x;
00386 _xtBZhBx(2,j) = _xtB*_ZHest.block<3,3>(6,3*j)*B*x;
00387 }
00388 }
00389
00390 _Q = 2*_xtBJ-_xtBZBx;
00391 double factor = -(lfd2/2)*likelihood;
00392 score_gradient += _Q*factor;
00393
00394 if(computeHessian)
00395 {
00396 Hessian += factor*(2*_Jest.transpose()*B*_Jest+2*_xtBH -_xtBZhBx -2*_xtBZBJ.transpose()
00397 -2*_xtBZBJ +_xtBZBZBx +_xtBZBZBx.transpose() -lfd2*_Q*_Q.transpose()/2 );
00398
00399 }
00400 return true;
00401 }
00402
00403
00404 template <typename PointSource, typename PointTarget>
00405 bool NDTMatcherD2D_2D<PointSource,PointTarget>::update_gradient_hessian_2d(
00406 Eigen::MatrixXd &score_gradient,
00407 Eigen::MatrixXd &Hessian,
00408 const Eigen::Vector3d & x,
00409 const Eigen::Matrix3d & B,
00410 const double &likelihood,
00411 bool computeHessian)
00412 {
00413
00414 xtBJ.setZero();
00415 xtBZBx.setZero();
00416 Q.setZero();
00417 JtBJ.setZero();
00418 xtBZBJ.setZero();
00419 xtBH.setZero();
00420 xtBZBZBx.setZero();
00421 xtBZhBx.setZero();
00422 TMP1.setZero();
00423 xtB.setZero();
00424
00425 xtB = x.transpose()*B;
00426 xtBJ = xtB*Jest;
00427
00428 TMP1 = xtB*Zest.block<3,3>(0,6)*B;
00429 xtBZBx(2) = TMP1*x;
00430 if(computeHessian)
00431 {
00432 xtBZBJ.col(2) = (TMP1*Jest).transpose();
00433 for(unsigned int j=0; j<3; j++)
00434 {
00435 xtBH(2,j) = xtB*Hest.block<3,1>(6,j);
00436 xtBZBZBx(2,j) = TMP1*Zest.block<3,3>(0,3*j)*B*x;
00437 xtBZhBx(2,j) = xtB*ZHest.block<3,3>(6,3*j)*B*x;
00438 }
00439 }
00440 Q = 2*xtBJ-xtBZBx;
00441 double factor = -(lfd2/2)*likelihood;
00442 score_gradient += Q*factor;
00443
00444 if(computeHessian)
00445 {
00446 Hessian += factor*(2*Jest.transpose()*B*Jest+2*xtBH -xtBZhBx -2*xtBZBJ.transpose()
00447 -2*xtBZBJ +xtBZBZBx +xtBZBZBx.transpose() -lfd2*Q*Q.transpose()/2 );
00448
00449 }
00450 return true;
00451 }
00452
00453
00454 template <typename PointSource, typename PointTarget>
00455 void NDTMatcherD2D_2D<PointSource,PointTarget>::computeDerivativesLocal_2d(Eigen::Vector3d &x, Eigen::Matrix3d C1,
00456 Eigen::Matrix<double,3,3> &_Jest,
00457 Eigen::Matrix<double,9,3> &_Hest,
00458 Eigen::Matrix<double,3,9> &_Zest,
00459 Eigen::Matrix<double,9,9> &_ZHest,
00460 bool computeHessian)
00461 {
00462 _Jest(0,2) = -x(1);
00463 _Jest(1,2) = x(0);
00464
00465 Eigen::Matrix3d myBlock;
00466
00467 _Zest.block<3,3>(0,6)<<
00468 -2*C1(0,1), -C1(1,1) + C1(0,0), -C1(1,2),
00469 -C1(1,1) + C1(0,0), 2*C1(0,1), C1(0,2),
00470 -C1(1,2), C1(0,2), 0;
00471
00472 if(computeHessian)
00473 {
00474 _Hest.block<3,1>(6,2)<<-x(0),-x(1),0;
00475 _ZHest.block<3,3>(6,6)<<
00476 2*C1(1,1) - 2*C1(0,0), -4*C1(0,1), -C1(0,2),
00477 -4*C1(0,1), 2*C1(0,0) - 2*C1(1,1), -C1(1,2),
00478 -C1(0,2), -C1(1,2), 0;
00479 }
00480 }
00481
00482 template <typename PointSource, typename PointTarget>
00483 void NDTMatcherD2D_2D<PointSource,PointTarget>::computeDerivatives_2d(Eigen::Vector3d &x, Eigen::Matrix3d C1, bool computeHessian)
00484 {
00485
00486 Jest(0,2) = -x(1);
00487 Jest(1,2) = x(0);
00488
00489 Eigen::Matrix3d myBlock;
00490
00491 Zest.block<3,3>(0,6)<<
00492 -2*C1(0,1), -C1(1,1) + C1(0,0), -C1(1,2),
00493 -C1(1,1) + C1(0,0), 2*C1(0,1), C1(0,2),
00494 -C1(1,2), C1(0,2), 0;
00495
00496 if(computeHessian)
00497 {
00498 Hest.block<3,1>(6,2)<<-x(0),-x(1),0;
00499 ZHest.block<3,3>(6,6)<<
00500 2*C1(1,1) - 2*C1(0,0), -4*C1(0,1), -C1(0,2),
00501 -4*C1(0,1), 2*C1(0,0) - 2*C1(1,1), -C1(1,2),
00502 -C1(0,2), -C1(1,2), 0;
00503 }
00504 }
00505
00509
00510 #define USE_OMP_NDT_MATCHER_D2D_2D
00511 template <typename PointSource, typename PointTarget>
00512
00513 double NDTMatcherD2D_2D<PointSource,PointTarget>::derivativesNDT_2d(
00514 const std::vector<NDTCell<PointSource>*> &sourceNDT,
00515 const NDTMap<PointTarget> &targetNDT,
00516 Eigen::MatrixXd &score_gradient,
00517 Eigen::MatrixXd &Hessian,
00518 bool computeHessian
00519 )
00520 {
00521
00522
00523 double score_here = 0;
00524 int n_dimensions = score_gradient.rows();
00525
00526
00527 NUMBER_OF_ACTIVE_CELLS = 0;
00528 score_gradient.setZero();
00529 Hessian.setZero();
00530
00531 #ifdef USE_OMP_NDT_MATCHER_D2D_2D
00532 Eigen::MatrixXd score_gradient_omp;
00533 Eigen::MatrixXd score_here_omp;
00534 Eigen::MatrixXd Hessian_omp;
00535
00536 #define N_THREADS_2D 2
00537
00538
00539 score_gradient_omp.resize(n_dimensions,N_THREADS_2D);
00540 score_here_omp.resize(1,N_THREADS_2D);
00541 Hessian_omp.resize(n_dimensions,n_dimensions*N_THREADS_2D);
00542
00543 score_gradient_omp.setZero();
00544 score_here_omp.setZero();
00545 Hessian_omp.setZero();
00546
00547
00548 #pragma omp parallel num_threads(N_THREADS_2D)
00549 {
00550 #pragma omp for
00551 for(unsigned int i=0; i<sourceNDT.size(); i++)
00552 {
00553 PointTarget point;
00554 Eigen::Vector3d transformed;
00555 Eigen::Vector3d meanMoving, meanFixed;
00556 Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
00557 Eigen::MatrixXd score_gradient_omp_loc(n_dimensions,1);
00558 Eigen::MatrixXd Hessian_omp_loc(n_dimensions,n_dimensions);
00559 Eigen::Matrix<double,3,3> _Jest;
00560 Eigen::Matrix<double,9,3> _Hest;
00561 Eigen::Matrix<double,3,9> _Zest;
00562 Eigen::Matrix<double,9,9> _ZHest;
00563 double score_here_loc=0;
00564 int thread_id = omp_get_thread_num();
00565 NDTCell<PointTarget> *cell;
00566 bool exists = false;
00567 double det = 0;
00568
00569
00570 score_gradient_omp_loc.setZero();
00571 Hessian_omp_loc.setZero();
00572 _Jest.setZero();
00573 _Jest.block<2,2>(0,0).setIdentity();
00574 _Hest.setZero();
00575 _Zest.setZero();
00576 _ZHest.setZero();
00577
00578 meanMoving = sourceNDT[i]->getMean();
00579 CMoving= sourceNDT[i]->getCov();
00580 computeDerivativesLocal_2d(meanMoving, CMoving, _Jest, _Hest, _Zest, _ZHest, computeHessian);
00581
00582 point.x = meanMoving(0);
00583 point.y = meanMoving(1);
00584 point.z = meanMoving(2);
00585 std::vector<NDTCell<PointSource>*> cells = targetNDT.getCellsForPoint(point,2);
00586 for(unsigned int j=0; j<cells.size(); j++)
00587 {
00588 cell = cells[j];
00589 if(cell == NULL)
00590 {
00591 continue;
00592 }
00593 if(cell->hasGaussian_)
00594 {
00595 transformed = meanMoving - cell->getMean();
00596 CFixed = cell->getCov();
00597 CSum = (CFixed+CMoving);
00598 CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
00599 if(!exists)
00600 {
00601 continue;
00602 }
00603 double l = (transformed).dot(Cinv*(transformed));
00604 if(l*0 != 0)
00605 {
00606 continue;
00607 }
00608
00609 double sh = -lfd1*(exp(-lfd2*l/2));
00610 if(!update_gradient_hessian_local_2d(score_gradient_omp_loc,Hessian_omp_loc,transformed, Cinv, sh,
00611 _Jest, _Hest, _Zest, _ZHest, computeHessian))
00612 {
00613 continue;
00614 }
00615 score_here_loc += sh;
00616 cell = NULL;
00617 }
00618 }
00619
00620 score_gradient_omp.col(thread_id) += score_gradient_omp_loc;
00621 Hessian_omp.block(0,n_dimensions*thread_id,n_dimensions,n_dimensions) += Hessian_omp_loc;
00622 score_here_omp(0,thread_id) += score_here_loc;
00623
00624 }
00625 }
00626
00627
00628
00629 score_gradient = score_gradient_omp.rowwise().sum();
00630 score_here = score_here_omp.sum();
00631 if(computeHessian)
00632 {
00633
00634 for(int i=0; i<N_THREADS_2D; ++i)
00635 {
00636 Hessian += Hessian_omp.block(0,n_dimensions*i,n_dimensions,n_dimensions);
00637 }
00638 }
00639
00640 #else
00641 PointTarget point;
00642 Eigen::Vector3d transformed;
00643 Eigen::Vector3d meanMoving, meanFixed;
00644 Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
00645 NDTCell<PointTarget> *cell;
00646 bool exists = false;
00647 double det = 0;
00648 for(unsigned int i=0; i<sourceNDT.size(); i++)
00649 {
00650 meanMoving = sourceNDT[i]->getMean();
00651 CMoving= sourceNDT[i]->getCov();
00652 computeDerivatives_2d(meanMoving, CMoving, computeHessian);
00653
00654 point.x = meanMoving(0);
00655 point.y = meanMoving(1);
00656 point.z = meanMoving(2);
00657 std::vector<NDTCell<PointSource>*> cells = targetNDT.getCellsForPoint(point,2);
00658 for(int j=0; j<cells.size(); j++)
00659 {
00660 cell = cells[j];
00661 if(cell == NULL)
00662 {
00663 continue;
00664 }
00665 if(cell->hasGaussian_)
00666 {
00667 transformed = meanMoving - cell->getMean();
00668 CFixed = cell->getCov();
00669 CSum = (CFixed+CMoving);
00670 CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
00671 if(!exists)
00672 {
00673
00674 continue;
00675 }
00676 double l = (transformed).dot(Cinv*(transformed));
00677 if(l*0 != 0)
00678 {
00679
00680 continue;
00681 }
00682
00683 double sh = -lfd1*(exp(-lfd2*l/2));
00684
00685
00686
00687
00688
00689 if(!update_gradient_hessian_2d(score_gradient,Hessian,transformed, Cinv, sh, computeHessian))
00690 {
00691
00692 continue;
00693 }
00694
00695 score_here += sh;
00696
00697 cell = NULL;
00698 }
00699 }
00700 }
00701 #endif
00702
00703
00704
00705
00706 return score_here;
00707 }
00708
00709 template <typename PointSource, typename PointTarget>
00710 double NDTMatcherD2D_2D<PointSource,PointTarget>::lineSearch2D(
00711 Eigen::Matrix<double,3,1> &increment,
00712 std::vector<NDTCell<PointSource>*> &sourceNDT,
00713 NDTMap<PointTarget> &targetNDT
00714 )
00715 {
00716
00717
00718 double stp = 1.0;
00719 double recoverystep = 0.01;
00720 double dginit = 0.0;
00721 double ftol = 0.11111;
00722 double gtol = 0.99999;
00723 double stpmax = 4.0;
00724 double stpmin = 0.0001;
00725 int maxfev = 40;
00726 double xtol = 0.01;
00727
00728
00729 std::vector<NDTCell<PointSource>*> sourceNDTHere;
00730 double score_init = 0.0;
00731
00732 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> ps;
00733 ps.setIdentity();
00734
00735 Eigen::Matrix<double,3,1> scg_here;
00736 Eigen::MatrixXd pincr(3,1), score_gradient_here(3,1);
00737 Eigen::MatrixXd pseudoH(3,3);
00738 Eigen::Vector3d eulerAngles;
00740
00741 int info = 0;
00742 int infoc = 1;
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757 score_gradient_here.setZero();
00758 score_init = derivativesNDT_2d(sourceNDT,targetNDT,score_gradient_here,pseudoH,false);
00759 scg_here = score_gradient_here;
00760
00761
00762
00763
00764
00765 dginit = increment.dot(scg_here);
00766
00767
00768 if (dginit >= 0.0)
00769 {
00770 std::cout << "MoreThuente::cvsrch - wrong direction (dginit = " << dginit << ")" << std::endl;
00771
00772
00773
00774 increment = -increment;
00775 dginit = -dginit;
00776
00777 if (dginit >= 0.0)
00778 {
00779 for(unsigned int i=0; i<sourceNDTHere.size(); i++)
00780 {
00781 if(sourceNDTHere[i]!=NULL)
00782 delete sourceNDTHere[i];
00783 }
00784 return recoverystep;
00785 }
00786 }
00787 else
00788 {
00789
00790 }
00791
00792
00793
00794 bool brackt = false;
00795 bool stage1 = true;
00796 int nfev = 0;
00797 double dgtest = ftol * dginit;
00798 double width = stpmax - stpmin;
00799 double width1 = 2 * width;
00800
00801
00802
00803 double finit = 0.0;
00804 finit = score_init;
00805
00806
00807
00808
00809
00810
00811
00812
00813 double stx = 0.0;
00814 double fx = finit;
00815 double dgx = dginit;
00816 double sty = 0.0;
00817 double fy = finit;
00818 double dgy = dginit;
00819
00820
00821
00822
00823
00824
00825
00826
00827 double stmin, stmax;
00828 double fm, fxm, fym, dgm, dgxm, dgym;
00829
00830 while (1)
00831 {
00832
00833
00834 if (brackt)
00835 {
00836 stmin = MoreThuente::min(stx, sty);
00837 stmax = MoreThuente::max(stx, sty);
00838 }
00839 else
00840 {
00841 stmin = stx;
00842 stmax = stp + 4 * (stp - stx);
00843 }
00844
00845
00846 stp = MoreThuente::max(stp, stpmin);
00847 stp = MoreThuente::min(stp, stpmax);
00848
00849
00850
00851
00852 if ((brackt && ((stp <= stmin) || (stp >= stmax))) ||
00853 (nfev >= maxfev - 1) || (infoc == 0) ||
00854 (brackt && (stmax - stmin <= xtol * stmax)))
00855 {
00856 stp = stx;
00857 }
00858
00859
00860
00862
00863 pincr = stp*increment;
00864
00865 ps = Eigen::Translation<double,3>(pincr(0),pincr(1),0)*
00866 Eigen::AngleAxisd(pincr(2),Eigen::Vector3d::UnitZ());
00867
00868 for(unsigned int i=0; i<sourceNDTHere.size(); i++)
00869 {
00870 if(sourceNDTHere[i]!=NULL)
00871 delete sourceNDTHere[i];
00872 }
00873 sourceNDTHere.clear();
00874 for(unsigned int i=0; i<sourceNDT.size(); i++)
00875 {
00876 NDTCell<PointSource> *cell = sourceNDT[i];
00877 if(cell!=NULL)
00878 {
00879 Eigen::Vector3d mean = cell->getMean();
00880 Eigen::Matrix3d cov = cell->getCov();
00881 mean = ps*mean;
00882 cov = ps.rotation()*cov*ps.rotation().transpose();
00883 NDTCell<PointSource>* nd = (NDTCell<PointSource>*)cell->copy();
00884 nd->setMean(mean);
00885 nd->setCov(cov);
00886 sourceNDTHere.push_back(nd);
00887 }
00888 }
00889
00890 double f = 0.0;
00891 score_gradient_here.setZero();
00892
00893
00894
00895
00896
00897
00898
00899
00900 f = derivativesNDT_2d(sourceNDTHere,targetNDT,score_gradient_here,pseudoH,false);
00901
00902
00903
00904
00905
00906
00907 double dg = 0.0;
00908 scg_here = score_gradient_here;
00909
00910
00911
00912 dg = increment.dot(scg_here);
00913
00914
00915
00916
00917 nfev ++;
00918
00920
00921
00922
00923 double ftest1 = finit + stp * dgtest;
00924
00925
00926
00927
00928 if ((brackt && ((stp <= stmin) || (stp >= stmax))) || (infoc == 0))
00929 info = 6;
00930
00931 if ((stp == stpmax) && (f <= ftest1) && (dg <= dgtest))
00932 info = 5;
00933
00934 if ((stp == stpmin) && ((f > ftest1) || (dg >= dgtest)))
00935 info = 4;
00936
00937 if (nfev >= maxfev)
00938 info = 3;
00939
00940 if (brackt && (stmax-stmin <= xtol*stmax))
00941 info = 2;
00942
00943
00944 bool sufficientDecreaseTest = false;
00945 sufficientDecreaseTest = (f <= ftest1);
00946
00947
00948
00949
00950 if ((sufficientDecreaseTest) && (fabs(dg) <= gtol*(-dginit)))
00951 info = 1;
00952
00953 if (info != 0)
00954 {
00955 if (info != 1)
00956 {
00957
00958
00959
00960
00961 stp = recoverystep;
00962
00963
00964
00965
00966
00967 }
00968 else
00969 {
00970
00971 }
00972
00973
00974
00975
00976
00977 for(unsigned int i=0; i<sourceNDTHere.size(); i++)
00978 {
00979 if(sourceNDTHere[i]!=NULL)
00980 delete sourceNDTHere[i];
00981 }
00982
00983 return stp;
00984
00985 }
00986
00987
00988
00989
00990
00991
00992
00993 if (stage1 && (f <= ftest1) && (dg >= MoreThuente::min(ftol, gtol) * dginit))
00994 {
00995 stage1 = false;
00996 }
00997
00998
00999
01000
01001
01002
01003
01004 if (stage1 && (f <= fx) && (f > ftest1))
01005 {
01006
01007
01008
01009 fm = f - stp * dgtest;
01010 fxm = fx - stx * dgtest;
01011 fym = fy - sty * dgtest;
01012 dgm = dg - dgtest;
01013 dgxm = dgx - dgtest;
01014 dgym = dgy - dgtest;
01015
01016
01017
01018
01019
01020 infoc = MoreThuente::cstep(stx,fxm,dgxm,sty,fym,dgym,stp,fm,dgm,
01021 brackt,stmin,stmax);
01022
01023
01024
01025 fx = fxm + stx*dgtest;
01026 fy = fym + sty*dgtest;
01027 dgx = dgxm + dgtest;
01028 dgy = dgym + dgtest;
01029
01030 }
01031
01032 else
01033 {
01034
01035
01036
01037
01038
01039 infoc = MoreThuente::cstep(stx,fx,dgx,sty,fy,dgy,stp,f,dg,
01040 brackt,stmin,stmax);
01041
01042 }
01043
01044
01045
01046
01047 if (brackt)
01048 {
01049 if (fabs(sty - stx) >= 0.66 * width1)
01050 stp = stx + 0.5 * (sty - stx);
01051 width1 = width;
01052 width = fabs(sty-stx);
01053 }
01054
01055 }
01056
01057 }
01058
01059 template <typename PointSource, typename PointTarget>
01060 int NDTMatcherD2D_2D<PointSource,PointTarget>::MoreThuente::cstep(double& stx, double& fx, double& dx,
01061 double& sty, double& fy, double& dy,
01062 double& stp, double& fp, double& dp,
01063 bool& brackt, double stmin, double stmax)
01064 {
01065 int info = 0;
01066
01067
01068
01069 if ((brackt && ((stp <= MoreThuente::min(stx, sty)) || (stp >= MoreThuente::max(stx, sty)))) ||
01070 (dx * (stp - stx) >= 0.0) || (stmax < stmin))
01071 return info;
01072
01073
01074
01075 double sgnd = dp * (dx / fabs(dx));
01076
01077
01078
01079
01080
01081
01082 bool bound;
01083 double theta;
01084 double s;
01085 double gamma;
01086 double p,q,r;
01087 double stpc, stpq, stpf;
01088
01089 if (fp > fx)
01090 {
01091 info = 1;
01092 bound = 1;
01093 theta = 3 * (fx - fp) / (stp - stx) + dx + dp;
01094
01095
01096
01097 s = MoreThuente::absmax(theta, dx, dp);
01098 gamma = s * sqrt(((theta / s) * (theta / s)) - (dx / s) * (dp / s));
01099 if (stp < stx)
01100 gamma = -gamma;
01101
01102 p = (gamma - dx) + theta;
01103 q = ((gamma - dx) + gamma) + dp;
01104 r = p / q;
01105 stpc = stx + r * (stp - stx);
01106 stpq = stx + ((dx / ((fx - fp) / (stp - stx) + dx)) / 2) * (stp - stx);
01107 if (fabs(stpc - stx) < fabs(stpq - stx))
01108 stpf = stpc;
01109 else
01110 stpf = stpc + (stpq - stpc) / 2;
01111
01112 brackt = true;
01113 }
01114
01115
01116
01117
01118
01119
01120 else if (sgnd < 0.0)
01121 {
01122 info = 2;
01123 bound = false;
01124 theta = 3 * (fx - fp) / (stp - stx) + dx + dp;
01125 s = MoreThuente::absmax(theta,dx,dp);
01126 gamma = s * sqrt(((theta/s) * (theta/s)) - (dx / s) * (dp / s));
01127 if (stp > stx)
01128 gamma = -gamma;
01129 p = (gamma - dp) + theta;
01130 q = ((gamma - dp) + gamma) + dx;
01131 r = p / q;
01132 stpc = stp + r * (stx - stp);
01133 stpq = stp + (dp / (dp - dx)) * (stx - stp);
01134 if (fabs(stpc - stp) > fabs(stpq - stp))
01135 stpf = stpc;
01136 else
01137 stpf = stpq;
01138 brackt = true;
01139 }
01140
01141
01142
01143
01144
01145
01146
01147
01148
01149
01150 else if (fabs(dp) < fabs(dx))
01151 {
01152 info = 3;
01153 bound = true;
01154 theta = 3 * (fx - fp) / (stp - stx) + dx + dp;
01155 s = MoreThuente::absmax(theta, dx, dp);
01156
01157
01158
01159
01160 gamma = s * sqrt(max(0,(theta / s) * (theta / s) - (dx / s) * (dp / s)));
01161 if (stp > stx)
01162 gamma = -gamma;
01163
01164 p = (gamma - dp) + theta;
01165 q = (gamma + (dx - dp)) + gamma;
01166 r = p / q;
01167 if ((r < 0.0) && (gamma != 0.0))
01168 stpc = stp + r * (stx - stp);
01169 else if (stp > stx)
01170 stpc = stmax;
01171 else
01172 stpc = stmin;
01173
01174 stpq = stp + (dp/ (dp - dx)) * (stx - stp);
01175 if (brackt)
01176 {
01177 if (fabs(stp - stpc) < fabs(stp - stpq))
01178 stpf = stpc;
01179 else
01180 stpf = stpq;
01181 }
01182 else
01183 {
01184 if (fabs(stp - stpc) > fabs(stp - stpq))
01185 stpf = stpc;
01186 else
01187 stpf = stpq;
01188 }
01189 }
01190
01191
01192
01193
01194
01195
01196 else
01197 {
01198 info = 4;
01199 bound = false;
01200 if (brackt)
01201 {
01202 theta = 3 * (fp - fy) / (sty - stp) + dy + dp;
01203 s = MoreThuente::absmax(theta, dy, dp);
01204 gamma = s * sqrt(((theta/s)*(theta/s)) - (dy / s) * (dp / s));
01205 if (stp > sty)
01206 gamma = -gamma;
01207 p = (gamma - dp) + theta;
01208 q = ((gamma - dp) + gamma) + dy;
01209 r = p / q;
01210 stpc = stp + r * (sty - stp);
01211 stpf = stpc;
01212 }
01213 else if (stp > stx)
01214 stpf = stmax;
01215 else
01216 stpf = stmin;
01217 }
01218
01219
01220
01221
01222 if (fp > fx)
01223 {
01224 sty = stp;
01225 fy = fp;
01226 dy = dp;
01227 }
01228 else
01229 {
01230 if (sgnd < 0.0)
01231 {
01232 sty = stx;
01233 fy = fx;
01234 dy = dx;
01235 }
01236 stx = stp;
01237 fx = fp;
01238 dx = dp;
01239 }
01240
01241
01242
01243 stpf = MoreThuente::min(stmax, stpf);
01244 stpf = MoreThuente::max(stmin, stpf);
01245 stp = stpf;
01246 if (brackt && bound)
01247 {
01248 if (sty > stx)
01249 stp = min(stx + 0.66 * (sty - stx), stp);
01250 else
01251 stp = max(stx + 0.66 * (sty - stx), stp);
01252 }
01253
01254 return info;
01255
01256 }
01257
01258 template <typename PointSource, typename PointTarget>
01259 double NDTMatcherD2D_2D<PointSource,PointTarget>::MoreThuente::min(double a, double b)
01260 {
01261 return (a < b ? a : b);
01262 }
01263
01264 template <typename PointSource, typename PointTarget>
01265 double NDTMatcherD2D_2D<PointSource,PointTarget>::MoreThuente::max(double a, double b)
01266 {
01267 return (a > b ? a : b);
01268 }
01269
01270 template <typename PointSource, typename PointTarget>
01271 double NDTMatcherD2D_2D<PointSource,PointTarget>::MoreThuente::absmax(double a, double b, double c)
01272 {
01273 a = fabs(a);
01274 b = fabs(b);
01275 c = fabs(c);
01276
01277 if (a > b)
01278 return (a > c) ? a : c;
01279 else
01280 return (b > c) ? b : c;
01281 }
01282
01283 template <typename PointSource, typename PointTarget>
01284 double NDTMatcherD2D_2D<PointSource,PointTarget>::normalizeAngle(double a)
01285 {
01286
01287 return atan2(sin(a), cos(a));
01288
01289 }
01290
01291 #if 0
01292 template <typename PointSource, typename PointTarget>
01293 bool NDTMatcherD2D_2D<PointSource,PointTarget>::covariance( NDTMap<PointTarget>& targetNDT,
01294 NDTMap<PointSource>& sourceNDT,
01295 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
01296 Eigen::MatrixXd &cov
01297 )
01298 {
01299
01300 double sigmaS = (0.03)*(0.03);
01301 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR;
01302 TR.setIdentity();
01303
01304 std::vector<NDTCell<PointSource>*> sourceNDTN = sourceNDT.pseudoTransformNDT(T);
01305 std::vector<NDTCell<PointTarget>*> targetNDTN = targetNDT.pseudoTransformNDT(T);
01306
01307 Eigen::MatrixXd scg(6,1);
01308 int NM = sourceNDTN.size() + targetNDTN.size();
01309
01310 Eigen::MatrixXd Jdpdz(NM,6);
01311
01312 NDTCell<PointTarget> *cell;
01313 Eigen::Vector3d transformed;
01314 Eigen::Vector3d meanMoving, meanFixed;
01315 Eigen::Matrix3d CMoving, CFixed, Cinv;
01316 bool exists = false;
01317 double det = 0;
01318 Eigen::Matrix<double,6,1> ones;
01319 ones<<1,1,1,1,1,1;
01320
01321 derivativesNDT(sourceNDTN,targetNDT,scg,cov,true);
01322
01323 Eigen::Matrix3d Q;
01324 Jdpdz.setZero();
01325 Q.setZero();
01326
01327 PointTarget point;
01328
01329 for(int i=0; i<sourceNDTN.size(); i++)
01330 {
01331 meanMoving = sourceNDTN[i]->getMean();
01332 point.x = meanMoving(0);
01333 point.y = meanMoving(1);
01334 point.z = meanMoving(2);
01335
01336 if(!targetNDT.getCellForPoint(point,cell))
01337 {
01338 continue;
01339 }
01340 if(cell == NULL)
01341 {
01342 continue;
01343 }
01344 if(cell->hasGaussian_)
01345 {
01346
01347 meanFixed = cell->getMean();
01348 transformed = meanMoving-meanFixed;
01349 CFixed = cell->getCov();
01350 CMoving= sourceNDTN[i]->getCov();
01351
01352 (CFixed+CMoving).computeInverseAndDetWithCheck(Cinv,det,exists);
01353 if(!exists) continue;
01354
01355
01356 double factor = (-transformed.dot(Cinv*transformed)/2);
01357
01358 if(factor < -120)
01359 {
01360 continue;
01361 }
01362 factor = exp(lfd2*factor)/2;
01363 if(factor > 1 || factor < 0 || factor*0 !=0)
01364 {
01365 continue;
01366 }
01367
01368 Q = -sigmaS*Cinv*Cinv;
01369
01370 Eigen::Matrix<double,6,1> G, xtQJ;
01371
01372 G.setZero();
01373 for(int q=3; q<6; q++)
01374 {
01375 G(q) = -transformed.transpose()*Q*Zest.block<3,3>(0,3*q)*Cinv*transformed;
01376 G(q) = G(q) -transformed.transpose()*Cinv*Zest.block<3,3>(0,3*q)*Q*transformed;
01377 }
01378
01379 xtQJ = transformed.transpose()*Q*Jest;
01380
01381 double f1 = (transformed.transpose()*Q*transformed);
01382 G = G + xtQJ + (-lfd2/2)*f1*ones;
01383 G = G*factor*lfd1*lfd2/2;
01384
01385 Jdpdz.row(i) = G.transpose();
01386
01387 for(int j=0; j<targetNDTN.size(); j++)
01388 {
01389 if(targetNDTN[j]->getMean() == meanFixed)
01390 {
01391
01392 Jdpdz.row(j+sourceNDTN.size()) = Jdpdz.row(j+sourceNDTN.size())+G.transpose();
01393 continue;
01394 }
01395 }
01396
01397 cell = NULL;
01398 }
01399 }
01400
01401
01402
01403
01404 Eigen::MatrixXd JK(6,6);
01405 JK = sigmaS*Jdpdz.transpose()*Jdpdz;
01406
01407
01408
01409
01410 cov = cov.inverse()*JK*cov.inverse();
01411
01412
01413
01414 for(unsigned int q=0; q<sourceNDTN.size(); q++)
01415 {
01416 delete sourceNDTN[q];
01417 }
01418 sourceNDTN.clear();
01419
01420 return true;
01421 }
01422 template <typename PointSource, typename PointTarget>
01423 bool NDTMatcherD2D_2D<PointSource,PointTarget>::covariance( pcl::PointCloud<PointTarget>& target,
01424 pcl::PointCloud<PointSource>& source,
01425 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
01426 Eigen::MatrixXd &cov
01427 )
01428 {
01429
01430 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR;
01431 TR.setIdentity();
01432
01433 pcl::PointCloud<PointSource> sourceCloud = lslgeneric::transformPointCloud(T,source);
01434
01435 LazyGrid<PointSource> prototypeSource(resolutions.front());
01436 LazyGrid<PointTarget> prototypeTarget(resolutions.front());
01437
01438 NDTMap<PointTarget> targetNDT( &prototypeTarget );
01439 targetNDT.loadPointCloud( target );
01440 targetNDT.computeNDTCells();
01441
01442 NDTMap<PointSource> sourceNDT( &prototypeSource );
01443 sourceNDT.loadPointCloud( sourceCloud );
01444 sourceNDT.computeNDTCells();
01445
01446 this->covariance(targetNDT,sourceNDT,TR,cov);
01447
01448 return true;
01449 }
01450 #endif
01451
01452 }