Merge branch 'master' of gitsvn-nt.oru.se:software/ndt_core

ndt_registration/include/ndt_registration/registration.h

 #ifndef REGISTRATION_H
 #define REGISTRATION_H
-typedef enum regstatus{SUCCESS=0,FAILURE=1,MAX_ITR=2}regStatus;
+typedef enum regstatus{ SUCCESS=0, DELTA_SCORE_LIMIT=1, GRADIENT_VANISHED=2, MAX_ITR=3, WRONG_DIRECTION=4, FAILURE=5 }regStatus;
+/* SUCCESS = registration success due to any condition
+ * DELTA_SCORE_LIMIT = registration converged after delta score < th
+ * GRADIENT_VANISHED = converged, norm of gradient is small
+ * MAX_ITR = No convergence after max interations has been reached
+ * WRONG_DIRECTION = Moving in the direction of the gradient didn't improve score
+ * FAILURE = Unclassified major problem (unreliable)
+ * */
+
 #endif // REGISTRATION_H

ndt_registration/src/ndt_matcher_d2d.cpp

@@ -230,6 +230,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
           if(nextNDT[i]!=NULL)
             delete nextNDT[i];
         }
+        finalscore=score_here;
         return true;
       }
     }
@@ -246,6 +247,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
           delete nextNDT[i];
       }
       //	    std::cout<<"itr "<<itr_ctr<<" dScore "<< 0 <<std::endl;
+      finalscore=score_here;
       return true;
     }
     //	pose_increment_v.block<3,1>(0,0) = -Hessian.block<3,3>(0,0).ldlt().solve(score_gradient.block<3,1>(0,0));
@@ -313,6 +315,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
           delete nextNDT[i];
       }
       //	    std::cout<<"itr "<<itr_ctr<<" dScore "<< 0 <<std::endl;
+      finalscore=score_here;
       return true;
     }
     //	std::cout<<"score("<<itr_ctr+1<<") = "<<score_here<<";\n";
@@ -395,7 +398,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
     
     if(itr_ctr>0){
       if(convergence = ((pose_increment_v.norm()) < DELTA_SCORE))
-        status_=SUCCESS;
+        status_=DELTA_SCORE_LIMIT;
      }
     if(!convergence && itr_ctr>ITR_MAX)
     {
@@ -425,7 +428,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
   //std::cout<<"res "<<current_resolution<<" itr "<<itr_ctr<<std::endl;
   
   //    this->finalscore = score/NUMBER_OF_ACTIVE_CELLS;
-  
+  finalscore=score_here;
   return ret;
 }
 

ndt_registration/src/ndt_matcher_d2d_2d.cpp

@@ -176,25 +176,28 @@ bool NDTMatcherD2D_2D::match( NDTMap& targetNDT,
     double score_here = derivativesNDT_2d(nextNDT,targetNDT,score_gradient_2d,H,true);
     scg = score_gradient_2d;
     //temporarily commented std::cout<<"itr "<<itr_ctr<<" score "<<score_here<<std::endl;
-    if(score_here < score_best)
-    {
+    if(score_here < score_best){
       Tbest = T;
       score_best = score_here;
       //temporarily commented  std::cout<<"best score "<<score_best<<" at "<<itr_ctr<<std::endl;
     }
 
-    if (score_gradient_2d.norm()<= 10e-2*DELTA_SCORE)
-    {
+    if (score_gradient_2d.norm()<= 10e-2*DELTA_SCORE){
       std::cout<<"\%gradient vanished at itr: "<<itr_ctr<<", norm : " << score_gradient_2d.norm() << std::endl;
       for(unsigned int i=0; i<nextNDT.size(); i++)
       {
         if(nextNDT[i]!=NULL)
           delete nextNDT[i];
       }
-      if(score_here > score_best)
-      {
+      if(score_here > score_best){
         std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
         T = Tbest;
+        finalscore=score_best;
+        status_=WRONG_DIRECTION;
+      }
+      else{
+        finalscore=score_here;
+        status_=DELTA_SCORE_LIMIT;
       }
       return true;
     }
@@ -238,6 +241,7 @@ bool NDTMatcherD2D_2D::match( NDTMap& targetNDT,
         //std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
         T = Tbest;
       }
+      finalscore=score_here;
       return true;
     }
     if(step_control) {
@@ -296,7 +300,7 @@ bool NDTMatcherD2D_2D::match( NDTMap& targetNDT,
     if(nextNDT[i]!=NULL)
       delete nextNDT[i];
   }
-
+  finalscore=score_here;
   return ret;
 }
 

ndt_registration/src/ndt_matcher_d2d_sc.cpp

@@ -523,9 +523,8 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
         Hessian = evecs*Lam*(evecs.transpose());
       } else {
         if(score_here > score_best)
-        {
           T = Tbest;
-        }
+
         //de-alloc nextNDT
         for(unsigned int i=0; i<nextNDT.size(); i++)
         {
@@ -533,22 +532,21 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
             delete nextNDT[i];
         }
         status_=SUCCESS;
+        finalscore=score_here;
         return true;
       }
     }
-    if (score_gradient.norm()<= DELTA_SCORE)
-    {
-      if(score_here > score_best)
-      {
+    if (score_gradient.norm()<= DELTA_SCORE){
+      if(score_here > score_best){
         T = Tbest;
       }
       //de-alloc nextNDT
-      for(unsigned int i=0; i<nextNDT.size(); i++)
-      {
+      for(unsigned int i=0; i<nextNDT.size(); i++){
         if(nextNDT[i]!=NULL)
           delete nextNDT[i];
       }
-      status_=SUCCESS;
+      finalscore=score_here;
+      status_=DELTA_SCORE_LIMIT;
       return true;
     }
 
@@ -557,27 +555,26 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
     pose_increment_v = -Hessian.ldlt().solve(score_gradient);
 
     double dginit = pose_increment_v.dot(scg);
-    if(dginit > 0)
-    {
+    if(dginit > 0){
       //de-alloc nextNDT
-      if(score_here > score_best)
-      {
+      if(score_here > score_best){
         T = Tbest;
       }
-      for(unsigned int i=0; i<nextNDT.size(); i++)
-      {
+      for(unsigned int i=0; i<nextNDT.size(); i++){
         if(nextNDT[i]!=NULL)
           delete nextNDT[i];
       }
+      finalscore=score_here;
+      status_=SUCCESS;
       return true;
     }
     //check direction here:
 
-    if(step_control) {
+    if(step_control)
       step_size = lineSearchMTSC(pose_increment_v,nextNDT,targetNDT,pose_local_v, Q, T);
-    } else {
+    else
       step_size = 1;
-    }
+
     pose_increment_v = step_size*pose_increment_v;
 
     // const double max_step = 1.0;
@@ -604,8 +601,7 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
     pose_increment_v_Tframe(2) = Tposition_diff(2);
     pose_local_v += pose_increment_v_Tframe;
 
-    for(unsigned int i=0; i<nextNDT.size(); i++)
-    {
+    for(unsigned int i=0; i<nextNDT.size(); i++){
       //TRANSFORM
       Eigen::Vector3d meanC = nextNDT[i]->getMean();
       Eigen::Matrix3d covC = nextNDT[i]->getCov();
@@ -615,12 +611,11 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
       nextNDT[i]->setCov(covC);
     }
 
-    if(itr_ctr>0)
-    {
-      convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
+    if(itr_ctr>0){
+      if(convergence = ((pose_increment_v.norm()) < DELTA_SCORE))
+        status_=DELTA_SCORE_LIMIT;
     }
-    if(itr_ctr>ITR_MAX)
-    {
+    if(itr_ctr>ITR_MAX){
       convergence = true;
       ret = false;
       status_=MAX_ITR;
@@ -644,15 +639,13 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
 
   score_here += alpha*computeScoreMahalanobis(X,Q);
   if(score_here > score_best)
-  {
     T = Tbest;
-  }
-  for(unsigned int i=0; i<nextNDT.size(); i++)
-  {
+
+  for(unsigned int i=0; i<nextNDT.size(); i++){
     if(nextNDT[i]!=NULL)
       delete nextNDT[i];
   }
-
+  finalscore=score_here;
   return ret;
 }
 } // namespace