Fix: try to minimize drift when still

ibois-epfl · Jun 23, 2023 · 7850c80 · 7850c80
1 parent b494316
commit 7850c80
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 8 deletions.
diff --git a/src/basictypes/se3.h b/src/basictypes/se3.h
@@ -31,6 +31,7 @@ struct  se3{
     se3(){  for(int i=0;i<6;i++) rt[i]=std::numeric_limits<float>::quiet_NaN();}
     se3(float rx,float ry,float rz,float tx,float ty,float tz){   rt[0]=rx;rt[1]=ry;rt[2]=rz;rt[3]=tx;rt[4]=ty;rt[5]=tz;}
     se3(float v){  for(int i=0;i<6;i++) rt[i]=v;}
+    se3(const se3 &v){  for(int i=0;i<6;i++) rt[i]=v.rt[i];}
     se3(const float * v){ for(int i=0;i<6;i++) rt[i]=v[i];}
     se3(const cv::Mat &rt){  *this=convert(rt);}
     se3(const cv::Mat &r,const cv::Mat &t){  *this=convert(r,t);}

diff --git a/src/optimization/pnpsolver.cpp b/src/optimization/pnpsolver.cpp
@@ -176,13 +176,12 @@ int PnPSolver::solvePnp( const Frame &frame, std::shared_ptr<Map> TheMap, std::v
     optimizer.addVertex(G2oVertexCamera);
     // Set MapPoint vertices
 
-    const float Chi2D =  5.99;
-    const float Chi3D =  7.815;
-    const float Chi8D = 15.507;
-    const float thHuber2D =  sqrt(5.99);
-    const float thHuber3D= sqrt(7.815);
-    const float thHuber8D = sqrt(15.507);
-
+    const float Chi2D =  5.99 * 2;
+    const float Chi3D =  7.815 * 2;
+    const float Chi8D = 15.507 * 2;
+    const float thHuber2D =  sqrt(5.99 * 2);
+    const float thHuber3D= sqrt(7.815 * 2);
+    const float thHuber8D = sqrt(15.507 * 2);
 
     float fx=frame.imageParams.fx();
     float fy=frame.imageParams.fy();

diff --git a/src/utils/system.cpp b/src/utils/system.cpp
@@ -193,11 +193,13 @@ cv::Mat System::process(const Frame &frame) {
     else{
         //tracking mode
         if( currentState==STATE_TRACKING){
+            auto prevPose = se3(_curPose_f2g);
             _curKFRef=getBestReferenceFrame(_prevFrame,_curPose_f2g);
             _curPose_f2g=track(_cFrame,_curPose_f2g);
             _debug_msg_("current pose="<<_curPose_f2g);
             __TSLAM_TIMER_EVENT__("track");
-            if( !_curPose_f2g.isValid())
+            // if the pose is too far away, we are actually lost
+            if( !_curPose_f2g.isValid() || cv::norm(cv::Mat(prevPose - _curPose_f2g)) > 5.0f)
                 currentState=STATE_LOST;
         }
         //lost??