ndt_matcher_d2d.hpp
Go to the documentation of this file.
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 //#define DO_DEBUG_PROC
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; // Argggg!!! This is very important to have initiated! (one day debugging later) :-) TODO do we need to set it to anything better?
00038     lfd1 = 1; //lfd1/(double)sourceNDT.getMyIndex()->size(); //current_resolution*2.5;
00039     lfd2 = 0.05; //0.1/current_resolution;
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     //initial guess
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         //iterative regular grid
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             //transform moving
00132             T = Temp*T; //ORIGINAL
00133             //T = T*Temp;
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     //locals
00191     bool convergence = false;
00192     //double score=0;
00193     double DELTA_SCORE = 10e-4*current_resolution;
00194     //double DELTA_SCORE = 0.0005;
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); //column vectors, pose_increment_v(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     //locals
00276     bool convergence = false;
00277     //double score=0;
00278     double score_best = INT_MAX;
00279     //double DELTA_SCORE = 0.0005;
00280     //double NORM_MAX = current_resolution, ROT_MAX = M_PI/10; //
00281     int itr_ctr = 0;
00282     //double alpha = 0.95;
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); //column vectors, pose_increment_v(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     //std::cout<<"pose(:,"<<1<<") = ["<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<"]';\n";
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         //derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,true);
00309         scg = score_gradient;
00310 //      std::cout<<"itr "<<itr_ctr<<std::endl;
00311         if(score_here < score_best) 
00312         {
00313             Tbest = T;
00314             score_best = score_here;
00315 //          std::cout<<"best score "<<score_best<<" at "<<itr_ctr<<std::endl;
00316         }
00317 
00318 //      Hessian = Hessian + score_gradient.norm()*Eigen::Matrix<double,6,6>::Identity();
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)  //|| evals.minCoeff()) // < 10e-5*evals.maxCoeff()) 
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             //double regularizer = 0.001*maxCoeff - minCoeff;
00329             Eigen::Matrix<double,6,1> reg;
00330             //ugly
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 //            std::cerr<<"regularizing\n";
00337         }
00338 //      std::cout<<"s("<<itr_ctr+1<<") = "<<score_here<<";\n";
00339 //      std::cout<<"H(:,:,"<<itr_ctr+1<<")  =  ["<< Hessian<<"];\n"<<std::endl;                           //
00340 //      std::cout<<"grad (:,"<<itr_ctr+1<<")= ["<<score_gradient.transpose()<<"];"<<std::endl;         //
00341         if (score_gradient.norm()<= DELTA_SCORE)
00342         {
00343 //          std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00344 //            std::cout<<"\%gradient vanished\n";
00345             if(score_here > score_best) 
00346             {
00347 //              std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
00348                 T = Tbest;
00349             }
00350             //de-alloc nextNDT
00351             for(unsigned int i=0; i<nextNDT.size(); i++)
00352             {
00353                 if(nextNDT[i]!=NULL)
00354                     delete nextNDT[i];
00355             }
00356 //          std::cout<<"itr "<<itr_ctr<<" dScore "<< 0 <<std::endl;
00357             return true;
00358         }
00359 //      pose_increment_v.block<3,1>(0,0) = -Hessian.block<3,3>(0,0).ldlt().solve(score_gradient.block<3,1>(0,0));
00360 //      pose_increment_v.block<3,1>(3,0) = -Hessian.block<3,3>(3,3).ldlt().solve(score_gradient.block<3,1>(3,0));
00361         /*
00362                 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol (Hessian);
00363                 Eigen::Matrix<double,6,1> evals = Sol.eigenvalues().real();
00364                 if(evals.minCoeff() < 0 ) {
00365                     std::cout<<"\%negative hessian\n";
00366                     std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00367                     //de-alloc nextNDT
00368                     for(unsigned int i=0; i<nextNDT.size(); i++) {
00369                         if(nextNDT[i]!=NULL)
00370                             delete nextNDT[i];
00371                     }
00372                     return true;
00373                 } else {
00374                 }
00375         */
00376 
00377         pose_increment_v = -Hessian.ldlt().solve(score_gradient);
00378         //pose_increment_v =-0.0001*scg; //
00379         /*      if(itr_ctr%2) {
00380                     pose_increment_v(0) = 0;
00381                     pose_increment_v(1) = 0;
00382                     pose_increment_v(2) = 0;
00383                 } else {
00384                 pose_increment_v(3) = 0;
00385                 pose_increment_v(4) = 0;
00386                 pose_increment_v(5) = 0;
00387                 }
00388                 */
00389         /*
00390         weights = scg.array().abs();
00391         weights.block(0,0,3,1) /= weights.block(0,0,3,1).maxCoeff();
00392         weights.block(3,0,3,1) /= weights.block(3,0,3,1).maxCoeff();
00393         std::cout<<"w = ["<<weights<<"];\n";
00394         std::cout<<"pose_increment_v= ["<<pose_increment_v.transpose()<<"];"<<std::endl;       //
00395         pose_increment_v = weights*pose_increment_v.array();
00396         //if(pose_increment_v.norm() > 1)
00397         //    pose_increment_v.normalize();
00398         //scg.normalize();
00399         //pose_increment_v = -0.00001*scg;
00400          */
00401         /*
00402         pose_increment_v /= pv;
00403         */
00404 
00405         //pose_increment_v = Hessian.jacobiSvd(Eigen::ComputeFullU|Eigen::ComputeFullV).solve(-score_gradient);
00406         double dginit = pose_increment_v.dot(scg);
00407         if(dginit > 0)
00408         {
00409 //          std::cout<<"incr(:,"<<itr_ctr+1<<") = ["<<pose_increment_v.transpose()<<"]';\n";
00410 //          std::cout<<"\%dg  =  "<<dginit<<std::endl;     //
00411 //            std::cout<<"\%can't decrease in this direction any more, done \n";
00412             //de-alloc nextNDT
00413             if(score_here > score_best) 
00414             {
00415 //              std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
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 //          std::cout<<"itr "<<itr_ctr<<" dScore "<< 0 <<std::endl;
00424             return true;
00425         }
00426 //      std::cout<<"score("<<itr_ctr+1<<") = "<<score_here<<";\n";
00427         /*
00428                 */
00429 
00430 
00431 //      if(dginit > 0) {
00432 //          std::cout<<"pose_increment_v= ["<<pose_increment_v.transpose()<<"]"<<std::endl;       //
00433 //          std::cout<<"dg  =  "<<dginit<<std::endl;     //
00434 //      }
00435         /*
00436         */
00437         /*
00438                 std::cout<<"pose_increment_v= ["<<pose_increment_v.transpose()<<"]"<<std::endl;       //
00439                 pose_increment_v = Hessian.jacobiSvd(Eigen::ComputeFullU|Eigen::ComputeFullV).solve(-score_gradient);
00440                 std::cout<<"dg    "<<pose_increment_v.dot(score_gradient)<<std::endl;     //
00441                 //make the initial increment reasonable...
00442                 //cout<<"incr_init = ["<<pose_increment_v.transpose()<<"]"<<endl;
00443                 double pnorm = sqrt(pose_increment_v(0)*pose_increment_v(0) + pose_increment_v(1)*pose_increment_v(1)
00444                                     +pose_increment_v(2)*pose_increment_v(2));
00445                 if(pnorm > NORM_MAX) {
00446                     pose_increment_v(0) = NORM_MAX*pose_increment_v(0)/pnorm;
00447                     pose_increment_v(1) = NORM_MAX*pose_increment_v(1)/pnorm;
00448                     pose_increment_v(2) = NORM_MAX*pose_increment_v(2)/pnorm;
00449                 }
00450                 pose_increment_v(3) = normalizeAngle(pose_increment_v(3));
00451                 pose_increment_v(3) = (pose_increment_v(3) > ROT_MAX) ? ROT_MAX : pose_increment_v(3);
00452                 pose_increment_v(3) = (pose_increment_v(3) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(3);
00453                 pose_increment_v(4) = normalizeAngle(pose_increment_v(4));
00454                 pose_increment_v(4) = (pose_increment_v(4) > ROT_MAX) ? ROT_MAX : pose_increment_v(4);
00455                 pose_increment_v(4) = (pose_increment_v(4) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(4);
00456                 pose_increment_v(5) = normalizeAngle(pose_increment_v(5));
00457                 pose_increment_v(5) = (pose_increment_v(5) > ROT_MAX) ? ROT_MAX : pose_increment_v(5);
00458                 pose_increment_v(5) = (pose_increment_v(5) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(5);
00459         //      cout<<"H  =  ["<<Hessian<<"]"<<endl;
00460         //      cout<<"grad= ["<<score_gradient.transpose()<<"]"<<endl;
00461         //      cout<<"dg    "<<pose_increment_v.dot(score_gradient)<<endl;
00462 
00463         */
00464         //check direction here:
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         //std::cout<<"\%iteration "<<itr_ctr<<" pose norm "<<(pose_increment_v.norm())<<" score "<<score_here<<" step "<<step_size<<std::endl;
00473         /*
00474         pose_increment_v(2) = 0;
00475         pose_increment_v(3) = 0;
00476         pose_increment_v(4) = 0;
00477         pose_increment_v(5) = 0;
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         //std::cout<<"incr= ["<<pose_increment_v.transpose()<<"]"<<std::endl;
00488         //transform source NDT
00489         T = TR*T;
00490 //      std::cout<<"incr(:,"<<itr_ctr+1<<") = ["<<pose_increment_v.transpose()<<"]';\n";
00491 //      std::cout<<"pose(:,"<<itr_ctr+2<<") = ["<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<"]';\n";
00492 
00493         for(unsigned int i=0; i<nextNDT.size(); i++)
00494         {
00495             //TRANSFORM
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             //convergence = ((score_gradient.norm()) < DELTA_SCORE);
00508         }
00509         if(itr_ctr>ITR_MAX)
00510         {
00511             convergence = true;
00512             ret = false;
00513         }
00514         itr_ctr++;
00515         //step_size *= alpha;
00516         //std::cout<<"step size "<<step_size<<std::endl;
00517     }
00518     
00519 //    std::cout<<"itr "<<itr_ctr<<" dScore "<< pose_increment_v.norm()<<std::endl;
00520     //std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00521     score_gradient.setZero();
00522     double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,false);
00523     if(score_here > score_best) 
00524     {
00525 //      std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
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 //    std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00535 //    std::cout<<"grad(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00536     /*
00537         snprintf(fname,49,"final.wrl");
00538         fout = fopen(fname,"w");
00539         fprintf(fout,"#VRML V2.0 utf8\n");
00540         targetNDT.writeToVRML(fout,Eigen::Vector3d(1,0,0));
00541         for(unsigned int i=0; i<nextNDT.size(); i++)
00542         {
00543         if(nextNDT[i]!=NULL)
00544         {
00545             nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00546         }
00547         }
00548         fclose(fout);
00549     */
00550     //std::cout<<"res "<<current_resolution<<" itr "<<itr_ctr<<std::endl;
00551 
00552 //    this->finalscore = score/NUMBER_OF_ACTIVE_CELLS;
00553 
00554     return ret;
00555 }
00556 
00557 //old iteration of matcher, kept here for backup
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     //locals
00567     bool convergence = false;
00568     //double score=0;
00569     double DELTA_SCORE = 10e-3*current_resolution;
00570     double score_best = INT_MAX;
00571     //double DELTA_SCORE = 0.0005;
00572     //double NORM_MAX = current_resolution, ROT_MAX = M_PI/10; //
00573     int itr_ctr = 0;
00574     //double alpha = 0.95;
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); //column vectors, pose_increment_v(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 //    std::cout<<"pose(:,"<<1<<") = ["<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<"]';\n";
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         //derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,true);
00600         scg = score_gradient;
00601 //      std::cout<<"itr "<<itr_ctr<<std::endl;
00602         if(score_here < score_best) 
00603         {
00604             Tbest = T;
00605             score_best = score_here;
00606 //          std::cout<<"best score "<<score_best<<" at "<<itr_ctr<<std::endl;
00607         }
00608 
00609 //      Hessian = Hessian + score_gradient.norm()*Eigen::Matrix<double,6,6>::Identity();
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)  //|| evals.minCoeff()) // < 10e-5*evals.maxCoeff()) 
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             //double regularizer = 0.001*maxCoeff - minCoeff;
00620             Eigen::Matrix<double,6,1> reg;
00621             //ugly
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 //      std::cout<<"s("<<itr_ctr+1<<") = "<<score_here<<";\n";
00630 //      std::cout<<"H(:,:,"<<itr_ctr+1<<")  =  ["<< Hessian<<"];\n"<<std::endl;                           //
00631 //      std::cout<<"grad (:,"<<itr_ctr+1<<")= ["<<score_gradient.transpose()<<"];"<<std::endl;         //
00632         if (score_gradient.norm()<= DELTA_SCORE)
00633         {
00634 //          std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
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             //de-alloc nextNDT
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 //      pose_increment_v.block<3,1>(0,0) = -Hessian.block<3,3>(0,0).ldlt().solve(score_gradient.block<3,1>(0,0));
00651 //      pose_increment_v.block<3,1>(3,0) = -Hessian.block<3,3>(3,3).ldlt().solve(score_gradient.block<3,1>(3,0));
00652         /*
00653                 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol (Hessian);
00654                 Eigen::Matrix<double,6,1> evals = Sol.eigenvalues().real();
00655                 if(evals.minCoeff() < 0 ) {
00656                     std::cout<<"\%negative hessian\n";
00657                     std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00658                     //de-alloc nextNDT
00659                     for(unsigned int i=0; i<nextNDT.size(); i++) {
00660                         if(nextNDT[i]!=NULL)
00661                             delete nextNDT[i];
00662                     }
00663                     return true;
00664                 } else {
00665                 }
00666         */
00667 
00668         pose_increment_v = -Hessian.ldlt().solve(score_gradient);
00669         //pose_increment_v =-0.0001*scg; //
00670         /*      if(itr_ctr%2) {
00671                     pose_increment_v(0) = 0;
00672                     pose_increment_v(1) = 0;
00673                     pose_increment_v(2) = 0;
00674                 } else {
00675                 pose_increment_v(3) = 0;
00676                 pose_increment_v(4) = 0;
00677                 pose_increment_v(5) = 0;
00678                 }
00679                 */
00680         /*
00681         weights = scg.array().abs();
00682         weights.block(0,0,3,1) /= weights.block(0,0,3,1).maxCoeff();
00683         weights.block(3,0,3,1) /= weights.block(3,0,3,1).maxCoeff();
00684         std::cout<<"w = ["<<weights<<"];\n";
00685         std::cout<<"pose_increment_v= ["<<pose_increment_v.transpose()<<"];"<<std::endl;       //
00686         pose_increment_v = weights*pose_increment_v.array();
00687         //if(pose_increment_v.norm() > 1)
00688         //    pose_increment_v.normalize();
00689         //scg.normalize();
00690         //pose_increment_v = -0.00001*scg;
00691          */
00692         /*
00693         pose_increment_v /= pv;
00694         */
00695 
00696         //pose_increment_v = Hessian.jacobiSvd(Eigen::ComputeFullU|Eigen::ComputeFullV).solve(-score_gradient);
00697         double dginit = pose_increment_v.dot(scg);
00698         if(dginit > 0)
00699         {
00700 //          std::cout<<"incr(:,"<<itr_ctr+1<<") = ["<<pose_increment_v.transpose()<<"]';\n";
00701 //          std::cout<<"\%dg  =  "<<dginit<<std::endl;     //
00702             std::cout<<"\%can't decrease in this direction any more, done \n";
00703             //de-alloc nextNDT
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 //      std::cout<<"score("<<itr_ctr+1<<") = "<<score_here<<";\n";
00717         /*
00718                 */
00719 
00720 
00721 //      if(dginit > 0) {
00722 //          std::cout<<"pose_increment_v= ["<<pose_increment_v.transpose()<<"]"<<std::endl;       //
00723 //          std::cout<<"dg  =  "<<dginit<<std::endl;     //
00724 //      }
00725         /*
00726         */
00727         /*
00728                 std::cout<<"pose_increment_v= ["<<pose_increment_v.transpose()<<"]"<<std::endl;       //
00729                 pose_increment_v = Hessian.jacobiSvd(Eigen::ComputeFullU|Eigen::ComputeFullV).solve(-score_gradient);
00730                 std::cout<<"dg    "<<pose_increment_v.dot(score_gradient)<<std::endl;     //
00731                 //make the initial increment reasonable...
00732                 //cout<<"incr_init = ["<<pose_increment_v.transpose()<<"]"<<endl;
00733                 double pnorm = sqrt(pose_increment_v(0)*pose_increment_v(0) + pose_increment_v(1)*pose_increment_v(1)
00734                                     +pose_increment_v(2)*pose_increment_v(2));
00735                 if(pnorm > NORM_MAX) {
00736                     pose_increment_v(0) = NORM_MAX*pose_increment_v(0)/pnorm;
00737                     pose_increment_v(1) = NORM_MAX*pose_increment_v(1)/pnorm;
00738                     pose_increment_v(2) = NORM_MAX*pose_increment_v(2)/pnorm;
00739                 }
00740                 pose_increment_v(3) = normalizeAngle(pose_increment_v(3));
00741                 pose_increment_v(3) = (pose_increment_v(3) > ROT_MAX) ? ROT_MAX : pose_increment_v(3);
00742                 pose_increment_v(3) = (pose_increment_v(3) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(3);
00743                 pose_increment_v(4) = normalizeAngle(pose_increment_v(4));
00744                 pose_increment_v(4) = (pose_increment_v(4) > ROT_MAX) ? ROT_MAX : pose_increment_v(4);
00745                 pose_increment_v(4) = (pose_increment_v(4) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(4);
00746                 pose_increment_v(5) = normalizeAngle(pose_increment_v(5));
00747                 pose_increment_v(5) = (pose_increment_v(5) > ROT_MAX) ? ROT_MAX : pose_increment_v(5);
00748                 pose_increment_v(5) = (pose_increment_v(5) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(5);
00749         //      cout<<"H  =  ["<<Hessian<<"]"<<endl;
00750         //      cout<<"grad= ["<<score_gradient.transpose()<<"]"<<endl;
00751         //      cout<<"dg    "<<pose_increment_v.dot(score_gradient)<<endl;
00752 
00753         */
00754         //check direction here:
00755 
00756         step_size = lineSearchMT(pose_increment_v,nextNDT,targetNDT);
00757         //step_size = 1;
00758         pose_increment_v = step_size*pose_increment_v;
00759         //std::cout<<"\%iteration "<<itr_ctr<<" pose norm "<<(pose_increment_v.norm())<<" score "<<score_here<<" step "<<step_size<<std::endl;
00760         /*
00761         pose_increment_v(2) = 0;
00762         pose_increment_v(3) = 0;
00763         pose_increment_v(4) = 0;
00764         pose_increment_v(5) = 0;
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         //std::cout<<"incr= ["<<pose_increment_v.transpose()<<"]"<<std::endl;
00775         //transform source NDT
00776         T = TR*T;
00777 //      std::cout<<"incr(:,"<<itr_ctr+1<<") = ["<<pose_increment_v.transpose()<<"]';\n";
00778 //      std::cout<<"pose(:,"<<itr_ctr+2<<") = ["<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<"]';\n";
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             //convergence = ((score_gradient.norm()) < DELTA_SCORE);
00790         }
00791         if(itr_ctr>ITR_MAX)
00792         {
00793             convergence = true;
00794             ret = false;
00795         }
00796         itr_ctr++;
00797         //step_size *= alpha;
00798         //std::cout<<"step size "<<step_size<<std::endl;
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 //    std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00816 //    std::cout<<"grad(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00817     /*
00818         snprintf(fname,49,"final.wrl");
00819         fout = fopen(fname,"w");
00820         fprintf(fout,"#VRML V2.0 utf8\n");
00821         targetNDT.writeToVRML(fout,Eigen::Vector3d(1,0,0));
00822         for(unsigned int i=0; i<nextNDT.size(); i++)
00823         {
00824         if(nextNDT[i]!=NULL)
00825         {
00826             nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00827         }
00828         }
00829         fclose(fout);
00830     */
00831     //std::cout<<"res "<<current_resolution<<" itr "<<itr_ctr<<std::endl;
00832 
00833 //    this->finalscore = score/NUMBER_OF_ACTIVE_CELLS;
00834 
00835     return ret;
00836 }
00837 #endif
00838 
00839 //iteratively update the score gradient and hessian
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     //vars for gradient
00856     Eigen::Matrix<double,6,1> _xtBJ, _xtBZBx, _Q;
00857     //vars for hessian
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 ); // + Eigen::Matrix<double,6,6>::Identity();
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         double lmax = 1;
00914         if(fabsf(likelihood) > lmax) {
00915         return false;
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     std::cout<<"Zest = ["<<Zest<<"];\n";
00955     std::cout<<"Jest = ["<<Jest<<"];\n";
00956     std::cout<<"Q = ["<<Q.transpose()<<"]';\n";
00957     std::cout<<"xtBJ = ["<<xtBJ.transpose()<<"]';\n";
00958     std::cout<<"xtBZBx = ["<<xtBZBx.transpose()<<"]';\n";
00959     std::cout<<"Hest = ["<<Hest<<"];\n";
00960     std::cout<<"ZHest = ["<<ZHest<<"];\n";
00961     std::cout<<"xtBZBJ = ["<<xtBZBJ<<"];\n";
00962     std::cout<<"xtBH = ["<<xtBH<<"];\n";
00963     std::cout<<"xtBZBZBx = ["<<xtBZBZBx<<"];\n";
00964     std::cout<<"xtBZhBx = ["<<xtBZhBx<<"];\n";
00965     */
00966     //double factor = -(lfd2/2)*pow(1 - pow(likelihood/lmax,2),2);
00967     double factor = -(lfd2/2)*likelihood;
00968     sg = Q*factor;
00969 //    double weight = pow(1 - pow(likelihood,2),2)
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 ); // + Eigen::Matrix<double,6,6>::Identity();
00976         Hessian += Hloc;// + Eigen::Matrix<double,6,6>::Identity();
00977         //Hessian += Jest.transpose()*Jest;
00978     }
00979 
00980     /*
00981      std::cout<<"B = ["<<B<<"];\n x = ["<<x.transpose()<<"]';\n";
00982      std::cout<<"l = "<<likelihood<<";\nscg = ["
00983     <<sg.transpose()<<"]';\n H = ["
00984     << Hloc<<"];\n H2 = ["<<Jest.transpose()*Jest<<"];\n H3 = ["
00985     << sg*sg.transpose() <<"];\n";
00986      */
00987     return true;
00988 
00989 }
00990 
00991 //pre-computes the derivative matrices Jest, Hest, Zest, ZHest
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     //_Zest
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         //Hest
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         //_ZHest
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     //Zest
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         //Hest
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         //ZHest
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 //      eps<<0.01,0.01,0.01;
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); //targetNDT.getAllCells(); //
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));// + (eps).dot(Cinv*(eps));
01221                 if(l*0 != 0)
01222                 {
01223                     continue;
01224                 }
01225                 //if(l > 120) continue;
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                 //delete cell;
01234                 //std::cout << "had not gaussian!" << std::endl;
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 //      NDTCell<PointTarget> *cell;
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     //std::vector<NDTCell<PointSource>*> source = sourceNDT.getAllInitializedCells();
01261 
01262     //for(unsigned int i=0; i<source.size(); i++) {
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         //SWITCHME
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             //std::cout<<"o1 "<<o1<<" o2 "<<o2<<std::endl;
01289 
01290             if(source->hasGaussian_) {
01291                 meanMoving = source->getMean();
01292                 CMoving= source->getCov();
01293                 //point.x = meanMoving(0); point.y = meanMoving(1); point.z = meanMoving(2);
01294                 //targetNDT.getCellForPoint(point,cell,false);
01295                 if(cell->hasGaussian_) {
01296                     //both have Gaussians
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                     //std::cout<<"sh: "<<sh<<std::endl;
01313                     score_here += o1*o2*sh;
01314                     //SWITCHME
01315                     //source->consistency_score += o1*o2*sh;
01316                     source->consistency_score += -o1*o2*sh;
01317                 }
01318             }
01319             //add the part of score updates based on occupancy alone
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             //std::cout<<"lam "<<lambda<<" comp "<<-lambda*((1-o1)*(1-o2) - o1*(1-o2) -o2*(1-o1))<<std::endl;
01326             score_here += -lambda*((1-o1)*(1-o2) - o1*(1-o2) -o2*(1-o1));
01327 
01328             //SWITCHME
01329             //source->consistency_score += -lambda*((1-o1)*(1-o2) - o1*(1-o2) -o2*(1-o1));
01330             double ll = cell->isInside(cen2) ? 1 : 0;
01331             //std::cout<<"j "<<j<<" sc "<<source->consistency_score<<" ll "<<ll<<" incr "<<ll*((1-o1)*(1-o2) - o1*(1-o2) -o2*(1-o1))<<std::endl;
01332             source->consistency_score += ll*((1-o1)*(1-o2) - o1*(1-o2) -o2*(1-o1));
01333         }
01334         it++;
01335     }
01336 //cleanup
01337     /*
01338     for(unsigned int i=0; i<source.size(); i++) {
01339         if(source[i]==NULL) {
01340         continue;
01341         }
01342         delete source[i];
01343         source[i] = NULL;
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             //fprintf(stderr," '[i]=%lf' ", (1.0-sh));
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 //compute the score gradient of a point cloud + transformation to an NDT
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 //    struct timeval tv_start, tv_end;
01449     double score_here = 0;
01450     int n_dimensions = score_gradient.rows();
01451 
01452 //    gettimeofday(&tv_start,NULL);
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     //n_threads = omp_get_num_threads();
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     //std::cout<<n_threads<<" "<<omp_get_thread_num()<<std::endl;
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); //targetNDT.getAllCells(); //
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                     //if(l > 120) continue;
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             //score_gradient_omp.block(0,thread_id,n_dimensions,1) += score_gradient_omp_loc;
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     } //end pragma block
01554     //std::cout<<"sgomp: "<<score_gradient_omp<<std::endl;
01555     //std::cout<<"somp: "<<score_here_omp<<std::endl;
01556 
01557     score_gradient = score_gradient_omp.rowwise().sum();
01558     score_here = score_here_omp.sum();
01559     if(computeHessian)
01560     {
01561         //std::cout<<"Homp: "<<Hessian_omp<<std::endl;
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); //targetNDT.getAllCells(); //
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                     //delete cell;
01601                     continue;
01602                 }
01603                 double l = (transformed).dot(Cinv*(transformed));
01604                 if(l*0 != 0)
01605                 {
01606                     //delete cell;
01607                     continue;
01608                 }
01609                 //if(l > 120) continue;
01610                 double sh = -lfd1*(exp(-lfd2*l/2));
01611                 //compute Jest, Hest, Zest, ZHest
01612                 //update score gradient
01613                 //std::cout<<"m1 = ["<<meanMoving.transpose()<<"]';\n m2 = ["<<cell->getMean().transpose()<<"]';\n";
01614                 //std::cout<<"C1 = ["<<CMoving<<"];\n C2 = ["<<CFixed<<"];\n";
01615 //                  if(!update_gradient_hessian(score_gradient, Hessian, transformed, Cinv, sh, computeHessian))
01616                 if(!update_gradient_hessian(score_gradient,Hessian,transformed, Cinv, sh, computeHessian))
01617                 {
01618                     //delete cell;
01619                     continue;
01620                 }
01621                 //NUMBER_OF_ACTIVE_CELLS++;
01622                 score_here += sh;
01623                 //delete cell;
01624                 cell = NULL;
01625             }
01626         }
01627     }
01628 #endif
01629     /*
01630     if(computeHessian) {
01631 
01632         //regularization 0.01*NUMBER_OF_ACTIVE_CELLS*
01633         //Hessian = Hessian + 0.1*NUMBER_OF_ACTIVE_CELLS*Eigen::Matrix<double,6,6>::Identity();
01634         Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol (Hessian);
01635         Eigen::Matrix<double,6,1> evals = Sol.eigenvalues().real();
01636 
01637         double minCoeff = evals.minCoeff();
01638         double maxCoeff = evals.maxCoeff();
01639 
01640         if(minCoeff < 0 ) { //evals.minCoeff() < 0.01*evals.maxCoeff()) {
01641         Eigen::Matrix<double,6,6> evecs = Sol.eigenvectors().real();
01642         double regularizer = 0.01*maxCoeff - minCoeff;
01643         Eigen::Matrix<double,6,1> reg;
01644         //ugly
01645         reg<<regularizer,regularizer,regularizer,regularizer,regularizer,regularizer;
01646         evals += reg;
01647         Eigen::Matrix<double,6,6> Lam;
01648         Lam = evals.asDiagonal();
01649         Hessian = evecs*Lam*(evecs.transpose());
01650         std::cerr<<"regularizing\n";
01651         }
01652         */
01653     /*
01654     if(minCoeff < 0.001*maxCoeff) {
01655     Eigen::Matrix<double,6,6> evecs = Sol.eigenvectors().real();
01656     for(int q=0;q<6;++q) {
01657         if(evals(q) < 0.001*maxCoeff) {
01658         evals(q)=0.001*maxCoeff;
01659         } else{
01660         break;
01661         }
01662     }
01663     Eigen::Matrix<double,6,6> Lam;
01664     Lam = evals.asDiagonal();
01665     Hessian = evecs*Lam*(evecs.transpose());
01666     std::cerr<<"BAD_HESSIAN\n";
01667     }
01668     }
01669     */
01670 
01671 //    gettimeofday(&tv_end,NULL);
01672 //    double time_load = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
01673 //    std::cout<<"3D::: time derivatives took is: "<<time_load<<std::endl;
01674     return score_here;
01675 }
01676 
01677 
01678 //perform line search to find the best descent rate (More&Thuente)
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     // default params
01688     double stp = 1.0; //default step
01689     double recoverystep = 0.1;
01690     double dginit = 0.0;
01691     double ftol = 0.11111; //epsilon 1
01692     double gtol = 0.99999; //epsilon 2
01693     double stpmax = 4.0;
01694     double stpmin = 0.001;
01695     int maxfev = 40; //max function evaluations
01696     double xtol = 0.01; //window of uncertainty around the optimal step
01697 
01698     //my temporary variables
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;                       // return code
01712     int infoc = 1;              // return code for subroutine cstep
01713 
01714     // Compute the initial gradient in the search direction and check
01715     // that s is a descent direction.
01716 
01717     //we want to maximize s, so we should minimize -s
01718     //score_init = scoreNDT(sourceNDT,targetNDT);
01719 
01720     //gradient directions are opposite for the negated function
01721     //score_gradient_init = -score_gradient_init;
01722 
01723 //  cout<<"score_init "<<score_init<<endl;
01724 //  cout<<"score_gradient_init "<<score_gradient_init.transpose()<<endl;
01725 //  cout<<"increment "<<increment.transpose()<<endl;
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 //  cout<<"dginit "<<dginit<<endl;
01732 
01733     if (dginit >= 0.0)
01734     {
01735         std::cout << "MoreThuente::cvsrch - wrong direction (dginit = " << dginit << ")" << std::endl;
01736         //return recoverystep; //TODO TSV -1; //
01737         //return -1;
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 //     cout<<"correct direction (dginit = " << dginit << ")" << endl;
01755     }
01756 
01757     // Initialize local variables.
01758 
01759     bool brackt = false;                // has the soln been bracketed?
01760     bool stage1 = true;         // are we in stage 1?
01761     int nfev = 0;                       // number of function evaluations
01762     double dgtest = ftol * dginit; // f for curvature condition
01763     double width = stpmax - stpmin; // interval width
01764     double width1 = 2 * width;  // ???
01765 
01766     //cout<<"dgtest "<<dgtest<<endl;
01767     // initial function value
01768     double finit = 0.0;
01769     finit = score_init;
01770 
01771     // The variables stx, fx, dgx contain the values of the step,
01772     // function, and directional derivative at the best step.  The
01773     // variables sty, fy, dgy contain the value of the step, function,
01774     // and derivative at the other endpoint of the interval of
01775     // uncertainty.  The variables stp, f, dg contain the values of the
01776     // step, function, and derivative at the current step.
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     // Get the linear solve tolerance for adjustable forcing term
01786     //double eta_original = -1.0;
01787     //double eta = 0.0;
01788     //eta = eta_original;
01789 
01790     // Start of iteration.
01791 
01792     double stmin, stmax;
01793     double fm, fxm, fym, dgm, dgxm, dgym;
01794 
01795     while (1)
01796     {
01797         // Set the minimum and maximum steps to correspond to the present
01798         // interval of uncertainty.
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         // Force the step to be within the bounds stpmax and stpmin.
01811         stp = MoreThuente::max(stp, stpmin);
01812         stp = MoreThuente::min(stp, stpmax);
01813 
01814         // If an unusual termination is to occur then let stp be the
01815         // lowest point obtained so far.
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         // Evaluate the function and gradient at stp
01825         // and compute the directional derivative.
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         /*f = scoreNDT(sourceNDT,targetNDT,ps);
01861         derivativesNDT(sourceNDT,targetNDT,ps,score_gradient_here,pseudoH,false);
01862         std::cout<<"scg1  " <<score_gradient_here.transpose()<<std::endl;
01863         */
01864 
01865         //option 2:
01866         //f = scoreNDT(sourceNDTHere,targetNDT);
01867         f = derivativesNDT(sourceNDTHere,targetNDT,score_gradient_here,pseudoH,false);
01868         //std::cout<<"scg2  " <<score_gradient_here.transpose()<<std::endl;
01869 
01870 
01871         //cout<<"incr " <<pincr.transpose()<<endl;
01872         //cout<<"score (f) "<<f<<endl;
01873 
01874         double dg = 0.0;
01875         scg_here = score_gradient_here;
01876         dg = increment.dot(scg_here);
01877 
01878 
01879         //VALGRIND_CHECK_VALUE_IS_DEFINED(dg);
01880         //cout<<"dg = "<<dg<<endl;
01881         nfev ++;
01882 
01884 
01885         //cout<<"consider step "<<stp<<endl;
01886         // Armijo-Goldstein sufficient decrease
01887         double ftest1 = finit + stp * dgtest;
01888         //cout<<"ftest1 is "<<ftest1<<endl;
01889 
01890         // Test for convergence.
01891 
01892         if ((brackt && ((stp <= stmin) || (stp >= stmax))) || (infoc == 0))
01893             info = 6;                   // Rounding errors
01894 
01895         if ((stp == stpmax) && (f <= ftest1) && (dg <= dgtest))
01896             info = 5;                   // stp=stpmax
01897 
01898         if ((stp == stpmin) && ((f > ftest1) || (dg >= dgtest)))
01899             info = 4;                   // stp=stpmin
01900 
01901         if (nfev >= maxfev)
01902             info = 3;                   // max'd out on fevals
01903 
01904         if (brackt && (stmax-stmin <= xtol*stmax))
01905             info = 2;                   // bracketed soln
01906 
01907         // RPP sufficient decrease test can be different
01908         bool sufficientDecreaseTest = false;
01909         sufficientDecreaseTest = (f <= ftest1);  // Armijo-Golstein
01910 
01911         //cout<<"ftest2 "<<gtol*(-dginit)<<endl;
01912         //cout<<"sufficientDecrease? "<<sufficientDecreaseTest<<endl;
01913         //cout<<"curvature ok? "<<(fabs(dg) <= gtol*(-dginit))<<endl;
01914         if ((sufficientDecreaseTest) && (fabs(dg) <= gtol*(-dginit)))
01915             info = 1;                   // Success!!!!
01916 
01917         if (info != 0)          // Line search is done
01918         {
01919             if (info != 1)              // Line search failed
01920             {
01921                 // RPP add
01922                 // counter.incrementNumFailedLineSearches();
01923 
01924                 //if (recoveryStepType == Constant)
01925                 stp = recoverystep;
01926 
01927                 //newgrp.computeX(oldgrp, dir, stp);
01928 
01929                 //message = "(USING RECOVERY STEP!)";
01930 
01931             }
01932             else                        // Line search succeeded
01933             {
01934                 //message = "(STEP ACCEPTED!)";
01935             }
01936 
01937             //print.printStep(nfev, stp, finit, f, message);
01938 
01939             // Returning the line search flag
01940             //cout<<"LineSearch::"<<message<<" info "<<info<<endl;
01941             for(unsigned int i=0; i<sourceNDTHere.size(); i++)
01942             {
01943                 if(sourceNDTHere[i]!=NULL)
01944                     delete sourceNDTHere[i];
01945             }
01946 //      std::cout<<"nfev = "<<nfev<<std::endl;
01947             return stp;
01948 
01949         } // info != 0
01950 
01951         // RPP add
01952         //counter.incrementNumIterations();
01953 
01954         // In the first stage we seek a step for which the modified
01955         // function has a nonpositive value and nonnegative derivative.
01956 
01957         if (stage1 && (f <= ftest1) && (dg >= MoreThuente::min(ftol, gtol) * dginit))
01958         {
01959             stage1 = false;
01960         }
01961 
01962         // A modified function is used to predict the step only if we have
01963         // not obtained a step for which the modified function has a
01964         // nonpositive function value and nonnegative derivative, and if a
01965         // lower function value has been obtained but the decrease is not
01966         // sufficient.
01967 
01968         if (stage1 && (f <= fx) && (f > ftest1))
01969         {
01970 
01971             // Define the modified function and derivative values.
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             // Call cstep to update the interval of uncertainty
01981             // and to compute the new step.
01982 
01983             //VALGRIND_CHECK_VALUE_IS_DEFINED(dgm);
01984             infoc = MoreThuente::cstep(stx,fxm,dgxm,sty,fym,dgym,stp,fm,dgm,
01985                                        brackt,stmin,stmax);
01986 
01987             // Reset the function and gradient values for f.
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             // Call cstep to update the interval of uncertainty
02000             // and to compute the new step.
02001 
02002             //VALGRIND_CHECK_VALUE_IS_DEFINED(dg);
02003             infoc = MoreThuente::cstep(stx,fx,dgx,sty,fy,dgy,stp,f,dg,
02004                                        brackt,stmin,stmax);
02005 
02006         }
02007 
02008         // Force a sufficient decrease in the size of the
02009         // interval of uncertainty.
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     } // while-loop
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     // Check the input parameters for errors.
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     // Determine if the derivatives have opposite sign.
02038 
02039     double sgnd = dp * (dx / fabs(dx));
02040 
02041     // First case. A higher function value.  The minimum is
02042     // bracketed. If the cubic step is closer to stx than the quadratic
02043     // step, the cubic step is taken, else the average of the cubic and
02044     // quadratic steps is taken.
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         //VALGRIND_CHECK_VALUE_IS_DEFINED(theta);
02059         //VALGRIND_CHECK_VALUE_IS_DEFINED(dx);
02060         //VALGRIND_CHECK_VALUE_IS_DEFINED(dp);
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     // Second case. A lower function value and derivatives of opposite
02080     // sign. The minimum is bracketed. If the cubic step is closer to
02081     // stx than the quadratic (secant) step, the cubic step is taken,
02082     // else the quadratic step is taken.
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     // Third case. A lower function value, derivatives of the same sign,
02106     // and the magnitude of the derivative decreases.  The cubic step is
02107     // only used if the cubic tends to infinity in the direction of the
02108     // step or if the minimum of the cubic is beyond stp. Otherwise the
02109     // cubic step is defined to be either stmin or stmax. The
02110     // quadratic (secant) step is also computed and if the minimum is
02111     // bracketed then the the step closest to stx is taken, else the
02112     // step farthest away is taken.
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         // The case gamma = 0 only arises if the cubic does not tend
02122         // to infinity in the direction of the step.
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     // Fourth case. A lower function value, derivatives of the same
02156     // sign, and the magnitude of the derivative does not decrease. If
02157     // the minimum is not bracketed, the step is either stmin or
02158     // stmax, else the cubic step is taken.
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     // Update the interval of uncertainty. This update does not depend
02184     // on the new step or the case analysis above.
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     // Compute the new step and safeguard it.
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     //set the angle between -M_PI and M_PI
02251     return atan2(sin(a), cos(a));
02252 
02253 }
02254 
02255 /*
02256 template <typename PointSource, typename PointTarget>
02257 void NDTMatcherD2D<PointSource,PointTarget>::generateScoreDebug(const char* out, pcl::PointCloud<pcl::PointXYZ>& fixed, pcl::PointCloud<pcl::PointXYZ>& moving) {
02258 
02259     std::ofstream lg(out,std::ios_base::out);
02260     int N_LINEAR = 100;
02261     int N_ROT    = 100;
02262     //lfd1 = 1;
02263     //lfd2 = 1;
02264 
02265     cout<<"generating scores...\n";
02266     for(current_resolution = 4; current_resolution>=0.5; current_resolution/=2) {
02267         cout<<"res "<<current_resolution<<endl;
02268         double lfc1,lfc2,lfd3;
02269         double integral, outlier_ratio, support_size;
02270         integral = 0.1;
02271         outlier_ratio = 0.3;
02272         support_size = current_resolution;
02273         lfc1 = (1-outlier_ratio)/integral;
02274         lfc2 = outlier_ratio/pow(support_size,3);
02275         lfd3 = -log(lfc2);
02276         lfd1 = -(-log( lfc1 + lfc2 ) - lfd3);
02277         lfd2 = -log((-log( lfc1 * exp( -0.5 ) + lfc2 ) - lfd3 ) / -lfd1);
02278 
02279         lfd1 = 1;//lfd1;
02280         lfd2 = 0.05;//0.8*lfd2;
02281 
02282         double lmin=-2, lmax=2, rmin=-M_PI/2, rmax=M_PI/2;
02283         double lstep = (lmax-lmin)/(N_LINEAR-1);
02284         double rstep = (rmax-rmin)/(N_ROT-1);
02285         Eigen::MatrixXd S(6,N_LINEAR);
02286         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;
02287         std::vector<NDTCell*> nextNDT;
02288 
02289         LazzyGrid prototype(current_resolution);
02290         NDTMap ndt( &prototype );
02291         ndt.loadPointCloud( fixed );
02292         ndt.computeNDTCells();
02293 
02294         double z=0, r=0, pitch=0, yaw=0;
02295         int k=0;
02296         for(double x=lmin; x<lmax; x+=lstep) {
02297             T = Eigen::Translation<double,3>(x,0,0);
02298             pcl::PointCloud<pcl::PointXYZ> cloud = moving;
02299             lslgeneric::transformPointCloudInPlace(T,cloud);
02300             NDTMap mov( &prototype );
02301             mov.loadPointCloud( cloud );
02302             mov.computeNDTCells();
02303             T.setIdentity();
02304             nextNDT = mov.pseudoTransformNDT(T);
02305 
02306             S(0,k) = scoreNDT(nextNDT,ndt);
02307             for(unsigned int i=0; i<nextNDT.size(); i++) {
02308                 if(nextNDT[i]!=NULL) delete nextNDT[i];
02309             }
02310             k++;
02311         }
02312         k=0;
02313         for(double x=lmin; x<lmax; x+=lstep) {
02314             T = Eigen::Translation<double,3>(0,x,0);
02315             pcl::PointCloud<pcl::PointXYZ> cloud = moving;
02316             lslgeneric::transformPointCloudInPlace(T,cloud);
02317             NDTMap mov( &prototype );
02318             mov.loadPointCloud( cloud );
02319             mov.computeNDTCells();
02320             T.setIdentity();
02321             nextNDT = mov.pseudoTransformNDT(T);
02322 
02323             S(1,k) = scoreNDT(nextNDT,ndt);
02324             for(unsigned int i=0; i<nextNDT.size(); i++) {
02325                 if(nextNDT[i]!=NULL) delete nextNDT[i];
02326             }
02327             k++;
02328         }
02329         k=0;
02330         for(double x=lmin; x<lmax; x+=lstep) {
02331             T = Eigen::Translation<double,3>(0,0,x);
02332             pcl::PointCloud<pcl::PointXYZ> cloud = moving;
02333             lslgeneric::transformPointCloudInPlace(T,cloud);
02334             NDTMap mov( &prototype );
02335             mov.loadPointCloud( cloud );
02336             mov.computeNDTCells();
02337             T.setIdentity();
02338             nextNDT = mov.pseudoTransformNDT(T);
02339 
02340             S(2,k) = scoreNDT(nextNDT,ndt);
02341             for(unsigned int i=0; i<nextNDT.size(); i++) {
02342                 if(nextNDT[i]!=NULL) delete nextNDT[i];
02343             }
02344             k++;
02345         }
02346 
02347         k=0;
02348         for(double r=rmin; r<rmax; r+=rstep) {
02349             T = Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitX()) *
02350                 Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitY()) *
02351                 Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitZ()) ;
02352             pcl::PointCloud<pcl::PointXYZ> cloud = moving;
02353             lslgeneric::transformPointCloudInPlace(T,cloud);
02354             NDTMap mov( &prototype );
02355             mov.loadPointCloud( cloud );
02356             mov.computeNDTCells();
02357             T.setIdentity();
02358             nextNDT = mov.pseudoTransformNDT(T);
02359 
02360             S(3,k) = scoreNDT(nextNDT,ndt);
02361             for(unsigned int i=0; i<nextNDT.size(); i++) {
02362                 if(nextNDT[i]!=NULL) delete nextNDT[i];
02363             }
02364             k++;
02365         }
02366         k=0;
02367         for(double r=rmin; r<rmax; r+=rstep) {
02368             T = Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitX()) *
02369                 Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitY()) *
02370                 Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitZ()) ;
02371             pcl::PointCloud<pcl::PointXYZ> cloud = moving;
02372             lslgeneric::transformPointCloudInPlace(T,cloud);
02373             NDTMap mov( &prototype );
02374             mov.loadPointCloud( cloud );
02375             mov.computeNDTCells();
02376             T.setIdentity();
02377             nextNDT = mov.pseudoTransformNDT(T);
02378 
02379             S(4,k) = scoreNDT(nextNDT,ndt);
02380             for(unsigned int i=0; i<nextNDT.size(); i++) {
02381                 if(nextNDT[i]!=NULL) delete nextNDT[i];
02382             }
02383             k++;
02384         }
02385         k=0;
02386         for(double r=rmin; r<rmax; r+=rstep) {
02387             T = Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitX()) *
02388                 Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitY()) *
02389                 Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitZ()) ;
02390             pcl::PointCloud<pcl::PointXYZ> cloud = moving;
02391             lslgeneric::transformPointCloudInPlace(T,cloud);
02392             NDTMap mov( &prototype );
02393             mov.loadPointCloud( cloud );
02394             mov.computeNDTCells();
02395             T.setIdentity();
02396             nextNDT = mov.pseudoTransformNDT(T);
02397 
02398             S(5,k) = scoreNDT(nextNDT,ndt);
02399             for(unsigned int i=0; i<nextNDT.size(); i++) {
02400                 if(nextNDT[i]!=NULL) delete nextNDT[i];
02401             }
02402             k++;
02403         }
02404 
02405         lg<<"Sf2f"<<(int)current_resolution<<" = ["<<S<<"];\n";
02406     }
02407     lg.close();
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); //column vectors
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     //TODO
02440     derivativesNDT(sourceNDTN,targetNDT,scg,cov,true);
02441 
02442     Eigen::Matrix3d Q;
02443     Jdpdz.setZero();
02444     Q.setZero();
02445 
02446     PointTarget point;
02447     //now compute Jdpdz
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             //compute Jdpdz.col(i)
02475             double factor = (-transformed.dot(Cinv*transformed)/2);
02476             //these conditions were copied from martin's code
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     //cout<<Jdpdz.transpose()<<endl;
02522 
02523     Eigen::MatrixXd JK(6,6);
02524     JK = sigmaS*Jdpdz.transpose()*Jdpdz;
02525 
02526     //cout<<"J*J'\n"<<JK<<endl;
02527     //cout<<"H\n"<<cov<<endl;
02528 
02529     cov = cov.inverse()*JK*cov.inverse();
02530     //cov = cov.inverse();//*fabsf(scoreNDT(sourceNDTN,targetNDT)*2/3);
02531     //cout<<"cov\n"<<cov<<endl;
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 }


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:32:03