ndt_matcher_d2d_feature.hpp
Go to the documentation of this file.
00001 #include <Eigen/Eigen>
00002 namespace lslgeneric
00003 {
00004 /*
00005 template <typename PointSource, typename PointTarget>
00006 bool
00007 NDTMatcherFeatureD2D<PointSource,PointTarget>::match( NDTMap<PointTarget>& targetNDT,
00008                              NDTMap<PointSource>& sourceNDT,
00009                              Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T)
00010 {
00011     Jest.setZero();
00012     Jest.template block<3,3>(0,0).setIdentity();
00013     Hest.setZero();
00014     Zest.setZero();
00015     ZHest.setZero();
00016 
00017     this->precomputeAngleDerivatives();
00018 
00019     lfd1 = 1;//lfd1/(double)sourceNDT.getMyIndex()->size(); //current_resolution*2.5;
00020     lfd2 = 0.05; //0.1/current_resolution;
00021 
00022     int ITR_MAX = 1000;
00023     bool convergence = false;
00024     double score=0;
00025     double scoreP=0;
00026     double DELTA_SCORE = 10e-5*this->current_resolution; //Henrik
00027     double NORM_MAX = 4*this->current_resolution, ROT_MAX = M_PI/4; //
00028     int itr_ctr = 0;
00029     double step_size = 1;
00030     Eigen::Matrix<double,6,1> pose_increment_v, pose_increment_reg_v, score_gradient; //column vectors
00031     Eigen::Matrix<double,6,6> Hessian;
00032     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR;
00033     Eigen::Vector3d transformed_vec, mean;
00034     bool ret = true;
00035     //T.setIdentity();
00036     TR.setIdentity();
00037 
00038     std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00039 
00040     while(!convergence) {
00041 
00042         TR.setIdentity();
00043         this->derivativesNDT(nextNDT,targetNDT,TR,score_gradient,Hessian,true);
00044         pose_increment_v = Hessian.ldlt().solve(-score_gradient);
00045         //pose_increment_v = Hessian.svd().solve(-score_gradient);
00046 
00047         double pnorm = sqrt(pose_increment_v(0)*pose_increment_v(0) + pose_increment_v(1)*pose_increment_v(1)
00048                             +pose_increment_v(2)*pose_increment_v(2));
00049         //cout << "pnorm : " << pnorm << endl;
00050         if(pnorm > NORM_MAX) {
00051             pose_increment_v(0) = NORM_MAX*pose_increment_v(0)/pnorm;
00052             pose_increment_v(1) = NORM_MAX*pose_increment_v(1)/pnorm;
00053             pose_increment_v(2) = NORM_MAX*pose_increment_v(2)/pnorm;
00054         }
00055         pose_increment_v(3) = normalizeAngle(pose_increment_v(3));
00056         pose_increment_v(3) = (pose_increment_v(3) > ROT_MAX) ? ROT_MAX : pose_increment_v(3);
00057         pose_increment_v(3) = (pose_increment_v(3) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(3);
00058         pose_increment_v(4) = normalizeAngle(pose_increment_v(4));
00059         pose_increment_v(4) = (pose_increment_v(4) > ROT_MAX) ? ROT_MAX : pose_increment_v(4);
00060         pose_increment_v(4) = (pose_increment_v(4) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(4);
00061         pose_increment_v(5) = normalizeAngle(pose_increment_v(5));
00062         pose_increment_v(5) = (pose_increment_v(5) > ROT_MAX) ? ROT_MAX : pose_increment_v(5);
00063         pose_increment_v(5) = (pose_increment_v(5) < -ROT_MAX) ? -ROT_MAX : pose_increment_v(5);
00064 
00065 //      pose_increment_v = pow(alfa,itr_ctr)*pose_increment_v;
00066         TR.setIdentity();
00067         TR =  Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00068             Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00069             Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00070             Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00071 
00072         step_size = this->lineSearchMT(score_gradient,pose_increment_v,nextNDT,TR,targetNDT);
00073         if(step_size < 0) {
00074             cout<<"can't decrease in this direction any more, done \n";
00075             return true;
00076         }
00077         pose_increment_v = step_size*pose_increment_v;
00078 
00079 
00080         TR.setIdentity();
00081         TR =  Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00082             Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00083             Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00084             Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00085         T = TR*T;
00086 
00087         for(unsigned int i=0; i<nextNDT.size(); i++) {
00088             if(nextNDT[i]!=NULL)
00089                 delete nextNDT[i];
00090         }
00091         nextNDT.clear();
00092         nextNDT = sourceNDT.pseudoTransformNDT(T);
00093 
00094         scoreP = score;
00095         score = scoreNDT(nextNDT,targetNDT);
00096 
00097         cout<<"iteration "<<itr_ctr<<" step "<<step_size<<" pose norm "<<(pose_increment_v.norm())<<" score_prev "<<scoreP<<" scoreN "<<score<<endl;
00098 
00099         if(itr_ctr>0) {
00100             convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
00101         }
00102         if(itr_ctr>ITR_MAX) {
00103             convergence = true;
00104             ret = false;
00105         }
00106         itr_ctr++;
00107 //      cout<<"step size "<<step_size<<endl;
00108     }
00109 
00110 //    cout<<"res "<<current_resolution<<" itr "<<itr_ctr<<endl;
00111 //    double scoreFinal = scoreNDT(nextNDT,targetNDT);
00112 //    cout<<"init "<<scoreInit<<" final "<<scoreFinal<<endl;
00113     for(unsigned int i=0; i<nextNDT.size(); i++) {
00114         if(nextNDT[i]!=NULL)
00115             delete nextNDT[i];
00116     }
00117 
00118     this->finalscore = score/NUMBER_OF_ACTIVE_CELLS;
00119 //    this->finalscore = score/(nextNDT.size()+targetNDT.getMyIndex()->size());
00120     cout<<"T: \n t = "<<T.translation().transpose()<<endl;
00121     cout<<"r= \n"<<T.rotation()<<endl;
00122     return ret;
00123 }
00124 */
00125 #if 0
00126 template <typename PointSource, typename PointTarget>
00127 bool NDTMatcherFeatureD2D<PointSource,PointTarget>::match( NDTMap<PointTarget>& targetNDT,
00128         NDTMap<PointSource>& sourceNDT,
00129         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T )
00130 {
00131 
00132     //locals
00133     bool convergence = false;
00134     double DELTA_SCORE = 10e-4;
00135     int ITR_MAX = 1000;
00136     int itr_ctr = 0;
00137     double step_size = 1;
00138     Eigen::Matrix<double,6,1>  pose_increment_v, scg;
00139     Eigen::MatrixXd Hessian(6,6), score_gradient(6,1); //column vectors, pose_increment_v(6,1)
00140 
00141     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR;
00142     Eigen::Vector3d transformed_vec, mean;
00143     bool ret = true;
00144 
00145 
00146     Eigen::Array<double,6,1> weights;
00147     while(!convergence)
00148     {
00149         std::vector<NDTCell<PointSource>*> nextNDT = sourceNDT.pseudoTransformNDT(T);
00150         TR.setIdentity();
00151         Hessian.setZero();
00152         score_gradient.setZero();
00153 
00154         double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,true);
00155         scg = score_gradient;
00156 
00157         Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol (Hessian);
00158         Eigen::Matrix<double,6,1> evals = Sol.eigenvalues().real();
00159         double minCoeff = evals.minCoeff();
00160         double maxCoeff = evals.maxCoeff();
00161         if(minCoeff < 0 )   //evals.minCoeff() < 0.01*evals.maxCoeff()) {
00162         {
00163             Eigen::Matrix<double,6,6> evecs = Sol.eigenvectors().real();
00164             double regularizer = 0.01*maxCoeff - minCoeff;
00165             Eigen::Matrix<double,6,1> reg;
00166             //ugly
00167             reg<<regularizer,regularizer,regularizer,regularizer,regularizer,regularizer;
00168             evals += reg;
00169             Eigen::Matrix<double,6,6> Lam;
00170             Lam = evals.asDiagonal();
00171             Hessian = evecs*Lam*(evecs.transpose());
00172             std::cerr<<"regularizing\n";
00173         }
00174 //      std::cout<<"s("<<itr_ctr+1<<") = "<<score_here<<";\n";
00175 //      std::cout<<"H(:,:,"<<itr_ctr+1<<")  =  ["<< Hessian<<"];\n"<<std::endl;                           //
00176 //      std::cout<<"grad (:,"<<itr_ctr+1<<")= ["<<score_gradient.transpose()<<"];"<<std::endl;         //
00177         if (score_gradient.norm()<= DELTA_SCORE)
00178         {
00179 //          std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00180             std::cout<<"\%gradient vanished\n";
00181             //de-alloc nextNDT
00182             for(unsigned int i=0; i<nextNDT.size(); i++)
00183             {
00184                 if(nextNDT[i]!=NULL)
00185                     delete nextNDT[i];
00186             }
00187 
00188             return true;
00189         }
00190         pose_increment_v = -Hessian.ldlt().solve(score_gradient);
00191         double dginit = pose_increment_v.dot(scg);
00192         if(dginit > 0)
00193         {
00194 //          std::cout<<"incr(:,"<<itr_ctr+1<<") = ["<<pose_increment_v.transpose()<<"]';\n";
00195 //          std::cout<<"\%dg  =  "<<dginit<<std::endl;     //
00196             std::cout<<"\%can't decrease in this direction any more, done \n";
00197             //de-alloc nextNDT
00198             for(unsigned int i=0; i<nextNDT.size(); i++)
00199             {
00200                 if(nextNDT[i]!=NULL)
00201                     delete nextNDT[i];
00202             }
00203             return true;
00204         }
00205         step_size = lineSearchMT(pose_increment_v,nextNDT,targetNDT);
00206         pose_increment_v = step_size*pose_increment_v;
00207 
00208         TR.setIdentity();
00209         TR =  Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00210               Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00211               Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00212               Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00213 
00214         T = TR*T;
00215 //      std::cout<<"incr(:,"<<itr_ctr+1<<") = ["<<pose_increment_v.transpose()<<"]';\n";
00216 //      std::cout<<"pose(:,"<<itr_ctr+2<<") = ["<<T.translation().transpose()<<" "<<T.rotation().eulerAngles(0,1,2).transpose()<<"]';\n";
00217 
00218         for(unsigned int i=0; i<nextNDT.size(); i++)
00219         {
00220             if(nextNDT[i]!=NULL)
00221                 delete nextNDT[i];
00222         }
00223 
00224         if(itr_ctr>0)
00225         {
00226             convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
00227         }
00228         if(itr_ctr>ITR_MAX)
00229         {
00230             convergence = true;
00231             ret = false;
00232         }
00233         itr_ctr++;
00234     }
00235 //    std::cout<<"incr(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00236 //    std::cout<<"grad(:,"<<itr_ctr+1<<") = [0 0 0 0 0 0]';\n";
00237     /*
00238         snprintf(fname,49,"final.wrl");
00239         fout = fopen(fname,"w");
00240         fprintf(fout,"#VRML V2.0 utf8\n");
00241         targetNDT.writeToVRML(fout,Eigen::Vector3d(1,0,0));
00242         for(unsigned int i=0; i<nextNDT.size(); i++)
00243         {
00244         if(nextNDT[i]!=NULL)
00245         {
00246             nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00247         }
00248         }
00249         fclose(fout);
00250     */
00251     //std::cout<<"res "<<current_resolution<<" itr "<<itr_ctr<<std::endl;
00252     return ret;
00253 }
00254 #endif
00255 
00256 template <typename PointSource, typename PointTarget>
00257 bool
00258 NDTMatcherFeatureD2D<PointSource,PointTarget>::covariance( NDTMap<PointTarget>& targetNDT,
00259         NDTMap<PointSource>& sourceNDT,
00260         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
00261         Eigen::Matrix<double,6,6> &cov)
00262 {
00263     assert(false);
00264 };
00265 
00266 //DEPRECATED???
00267 template <typename PointSource, typename PointTarget>
00268 double
00269 NDTMatcherFeatureD2D<PointSource,PointTarget>::scoreNDT(std::vector<NDTCell<PointSource>*> &sourceNDT, NDTMap<PointTarget> &targetNDT,
00270         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T)
00271 {
00272     NUMBER_OF_ACTIVE_CELLS = 0;
00273     double score_here = 0;
00274     double det = 0;
00275     bool exists = false;
00276     NDTCell<PointTarget> *cell;
00277     Eigen::Matrix3d covCombined, icov;
00278     Eigen::Vector3d meanFixed;
00279     Eigen::Vector3d meanMoving;
00280     Eigen::Matrix3d R = T.rotation();
00281     std::vector<std::pair<unsigned int, double> > scores;
00282     for(unsigned int j=0; j<_corr.size(); j++)
00283     {
00284         unsigned int i = _corr[j].second;
00285         if (_corr[j].second >= (int)sourceNDT.size())
00286         {
00287             std::cout << "second correspondance : " << _corr[j].second << ", " << sourceNDT.size() << std::endl;
00288         }
00289         if (sourceNDT[i] == NULL)
00290         {
00291             std::cout << "sourceNDT[i] == NULL!" << std::endl;
00292         }
00293         meanMoving = T*sourceNDT[i]->getMean();
00294 
00295         cell = targetNDT.getCellIdx(_corr[j].first);
00296         {
00297 
00298             if(cell == NULL)
00299             {
00300                 std::cout << "cell== NULL!!!" << std::endl;
00301             }
00302             else
00303             {
00304                 if(cell->hasGaussian_)
00305                 {
00306                     meanFixed = cell->getMean();
00307                     covCombined = cell->getCov() + R.transpose()*sourceNDT[i]->getCov()*R;
00308                     covCombined.computeInverseAndDetWithCheck(icov,det,exists);
00309                     if(!exists) continue;
00310                     double l = (meanMoving-meanFixed).dot(icov*(meanMoving-meanFixed));
00311                     if(l*0 != 0) continue;
00312                     if(l > 120) continue;
00313 
00314                     double sh = -lfd1*(exp(-lfd2*l/2));
00315 
00316                     if(fabsf(sh) > 1e-10)
00317                     {
00318                         NUMBER_OF_ACTIVE_CELLS++;
00319                     }
00320                     scores.push_back(std::pair<unsigned int, double>(j, sh));
00321                     score_here += sh;
00322                     //score_here += l;
00323                 }
00324             }
00325         }
00326     }
00327 
00328     if (_trimFactor == 1.)
00329     {
00330         return score_here;
00331     }
00332     else
00333     {
00334         // Determine the score value
00335         if (scores.empty()) // TODO, this happens(!), why??!??
00336             return score_here;
00337 
00338         score_here = 0.;
00339         unsigned int index = static_cast<unsigned int>(_trimFactor * (scores.size() - 1));
00340         //      std::nth_element (scores.begin(), scores.begin()+index, scores.end(), sort_scores()); //boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
00341         std::nth_element (scores.begin(), scores.begin()+index, scores.end(), boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
00342         std::fill(_goodCorr.begin(), _goodCorr.end(), false);
00343         //      std::cout << "_goodCorr.size() : " << _goodCorr.size() << " scores.size() : " << scores.size() << " index : " << index << std::endl;
00344         for (unsigned int i = 0; i < _goodCorr.size(); i++)
00345         {
00346             if (i <= index)
00347             {
00348                 score_here += scores[i].second;
00349                 _goodCorr[scores[i].first] = true;
00350             }
00351         }
00352         return score_here;
00353     }
00354 }
00355 
00356 template <typename PointSource, typename PointTarget>
00357 double
00358 NDTMatcherFeatureD2D<PointSource,PointTarget>::derivativesNDT( 
00359     const std::vector<NDTCell<PointSource>*> &sourceNDT,
00360     const NDTMap<PointTarget> &targetNDT,
00361     Eigen::MatrixXd &score_gradient,
00362     Eigen::MatrixXd &Hessian,
00363     bool computeHessian
00364 )
00365 {
00366 
00367     struct timeval tv_start, tv_end;
00368     double score_here = 0;
00369 
00370     gettimeofday(&tv_start,NULL);
00371     NUMBER_OF_ACTIVE_CELLS = 0;
00372     score_gradient.setZero();
00373     Hessian.setZero();
00374 
00375     PointTarget point;
00376     Eigen::Vector3d transformed;
00377     Eigen::Vector3d meanMoving, meanFixed;
00378     Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
00379     NDTCell<PointTarget> *cell;
00380     bool exists = false;
00381     double det = 0;
00382     for (unsigned int j = 0; j < _corr.size(); j++)
00383     {
00384         if (!_goodCorr[j])
00385             continue;
00386 
00387         unsigned int i = _corr[j].second;
00388         if (i >= sourceNDT.size())
00389         {
00390             std::cout << "sourceNDT.size() : " << sourceNDT.size() << ", i: " << i << std::endl;
00391         }
00392         assert(i < sourceNDT.size());
00393         
00394         meanMoving = sourceNDT[i]->getMean();
00395         CMoving= sourceNDT[i]->getCov();
00396         
00397         this->computeDerivatives(meanMoving, CMoving, computeHessian);
00398 
00399         point.x = meanMoving(0);
00400         point.y = meanMoving(1);
00401         point.z = meanMoving(2);
00402        
00403         cell = targetNDT.getCellIdx(_corr[j].first);
00404         if(cell == NULL)
00405         {
00406             continue;
00407         }
00408         if(cell->hasGaussian_)
00409         {
00410             transformed = meanMoving - cell->getMean();
00411             CFixed = cell->getCov();
00412             CSum = (CFixed+CMoving);
00413             CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
00414             if(!exists)
00415             {
00416                 //delete cell;
00417                 continue;
00418             }
00419             double l = (transformed).dot(Cinv*(transformed));
00420             if(l*0 != 0)
00421             {
00422                 //delete cell;
00423                 continue;
00424             }
00425             double sh = -lfd1*(exp(-lfd2*l/2));
00426             //std::cout<<"m1 = ["<<meanMoving.transpose()<<"]';\n m2 = ["<<cell->getMean().transpose()<<"]';\n";
00427             //std::cout<<"C1 = ["<<CMoving<<"];\n C2 = ["<<CFixed<<"];\n";
00428             //update score gradient
00429             if(!this->update_gradient_hessian(score_gradient,Hessian,transformed, Cinv, sh, computeHessian))
00430             {
00431                 continue;
00432             }
00433             score_here += sh;
00434             cell = NULL;
00435         }
00436     }
00437     gettimeofday(&tv_end,NULL);
00438 
00439     //double time_load = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
00440     //std::cout<<"time derivatives took is: "<<time_load<<std::endl;
00441     return score_here;
00442 
00443 //TSV: these are the old derivatives, in case i messed something up
00444 #if 0
00445     NDTCell<PointTarget> *cell;
00446     Eigen::Vector3d transformed;
00447     Eigen::Vector3d meanMoving, meanFixed;
00448     Eigen::Matrix3d CMoving, CFixed, Cinv, R;
00449     bool exists = false;
00450     double det = 0;
00451 
00452     R = transform.rotation();
00453 
00454     Jest.setZero();
00455     (Jest.template block<3,3>(0,0)).setIdentity();
00456     Hest.setZero();
00457     Zest.setZero();
00458     ZHest.setZero();
00459 
00460     PointSource point;
00461     score_gradient.setZero();
00462     Hessian.setZero();
00463 
00464     for (unsigned int j = 0; j < _corr.size(); j++)
00465     {
00466         if (!_goodCorr[j])
00467             continue;
00468 
00469         unsigned int i = _corr[j].second;
00470         if (i >= sourceNDT.size())
00471         {
00472             std::cout << "sourceNDT.size() : " << sourceNDT.size() << ", i: " << i << std::endl;
00473         }
00474         assert(i < sourceNDT.size());
00475         meanMoving = transform*sourceNDT[i]->getMean();
00476         point.x = meanMoving(0);
00477         point.y = meanMoving(1);
00478         point.z = meanMoving(2);
00479         transformed = meanMoving;
00480 
00481         cell = targetNDT.getCellIdx(_corr[j].first);
00482         {
00483             if(cell == NULL)
00484             {
00485                 continue;
00486             }
00487             if(cell->hasGaussian_)
00488             {
00489                 meanFixed = cell->getMean();
00490                 transformed -= meanFixed;
00491                 CFixed = cell->getCov();
00492                 CMoving= R*sourceNDT[i]->getCov()*R.transpose();
00493 
00494                 (CFixed+CMoving).computeInverseAndDetWithCheck(Cinv,det,exists);
00495                 if(!exists) continue;
00496 
00497                 //compute Jest, Hest, Zest, ZHest
00498                 this->computeDerivatives(meanMoving, CMoving);
00499 
00500                 //update score gradient
00501                 if(!update_gradient_hessian(score_gradient, Hessian, transformed, Cinv))
00502                 {
00503                     continue;
00504                 }
00505 
00506                 cell = NULL;
00507             }
00508         }
00509     }
00510 #endif
00511 }
00512 
00513 //TSV: use the standard ones
00514 #if 0
00515 template <typename PointSource, typename PointTarget>
00516 bool
00517 NDTMatcherFeatureD2D<PointSource,PointTarget>::update_gradient_hessian(
00518     Eigen::Matrix<double,6,1> &score_gradient,
00519     Eigen::Matrix<double,6,6> &Hessian,
00520     Eigen::Vector3d & x,
00521     Eigen::Matrix3d & B)
00522 {
00523 
00524     //vars for gradient
00525     Eigen::Matrix<double,6,1> xtBJ, xtBZBx, Q;
00526     //vars for hessian
00527     Eigen::Matrix<double,6,6> JtBJ, xtBZBJ, xtBH, xtBZBZBx, xtBZhBx;
00528     Eigen::Matrix<double,1,3> TMP1;
00529 
00530     double factor = (-x.dot(B*x)/2);
00531 
00532     //these conditions were copied from martin's code
00533     // Henrik, these conditions doesn't apply here if the initial estimate is too far away. One option could be to run a closed form solution step (ICP) first to avoid this problem.
00534     if(factor < -120)
00535     {
00536 //       std::cout << "factor : " << factor << std::endl;
00537 //       return false;
00538     }
00539 
00540     factor = exp(lfd2*factor)/2;
00541     if(factor > 1 || factor < 0 || factor*0 !=0)
00542     {
00543         std::cout << "factor > 1 || factor < 0 || factor*0 !=0" << std::endl;
00544         return false;
00545     }
00546 
00547     xtBJ = 2*x.transpose()*B*Jest;
00548 
00549     for(unsigned int i=0; i<6; i++)
00550     {
00551         TMP1 = x.transpose()*B*(Zest.template block<3,3>(0,3*i))*B;
00552         xtBZBx(i) = -TMP1*x;
00553         xtBZBJ.row(i) = -TMP1*Jest;
00554     }
00555     Q = xtBJ+xtBZBx;
00556 
00557     score_gradient += lfd1*lfd2*Q*factor;
00558 
00559 
00560     for(unsigned int i=0; i<6; i++)
00561     {
00562         for(unsigned int j=0; j<6; j++)
00563         {
00564             xtBH(i,j) = x.transpose()*B*(Hest.template block<3,1>(3*i,j));
00565             xtBZBZBx(i,j) = x.transpose()*B*(Zest.template block<3,3>(0,3*i))*B*(Zest.template block<3,3>(0,3*j))*B*x;
00566             xtBZhBx(i,j) = x.transpose()*B*(ZHest.template block<3,3>(3*j,3*i))*B*x;
00567         }
00568     }
00569 
00570     Hessian += lfd1*lfd2*2*factor*(Jest.transpose()*B*Jest - lfd2*Q*Q.transpose()/4 -
00571                                    2*xtBZBJ + xtBH - xtBZBZBx - xtBZhBx/2 );
00572 
00573     return true;
00574 
00575 }
00576 #endif
00577 
00578 }


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