From 7850c804ef676730be01e2239b30fe426c647772 Mon Sep 17 00:00:00 2001
From: Petingo <eyanghongbin0315@gmail.com>
Date: Fri, 23 Jun 2023 14:54:19 +0200
Subject: [PATCH] Fix: try to minimize drift when still

---
 src/basictypes/se3.h           |  1 +
 src/optimization/pnpsolver.cpp | 13 ++++++-------
 src/utils/system.cpp           |  4 +++-
 3 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/src/basictypes/se3.h b/src/basictypes/se3.h
index 251fc8d9..966a5e13 100644
--- 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
index 13770b2a..31c4ac81 100644
--- 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
index b5e870cc..29150460 100644
--- 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??