hack predicted velocity by differencing stereo odometry, test with st…

…ereo+imu data of both Tsukuba and DlLiLi
HaifengSu · May 21, 2017 · 9d63678 · 9d63678
1 parent 7dcee74
commit 9d63678
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 7 deletions.
diff --git a/data/settingfiles_stereo_imu/dilili.yaml b/data/settingfiles_stereo_imu/dilili.yaml
@@ -2,7 +2,7 @@
 startIndex: 9
 finishIndex: 399
 dataset: "DiLiLi" # choose one of KITTIOdoSeq, Tsukuba, MalagaUrbanExtract6, DiLiLi
-use_imu_data:false
+use_imu_data:true
 
 input_path: /media/jhuai/Seagate/data/DILILI-0510_square_400_no_calib/0510_square_400_no_calib
 time_file: /media/jhuai/Seagate/data/DILILI-0510_square_400_no_calib/0510_square_400_no_calib/video_ts.txt

diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
@@ -528,7 +528,7 @@ void KeyFrame::SetBadFlag()
 
         // Update Spanning Tree
         set<KeyFrame*> sParentCandidates;
-        sParentCandidates.insert(mpParent);
+        sParentCandidates.insert(mpParent);// TODO: mpParent may be NULL if the tracking experience difficulties
 
         // Assign at each iteration one children with a parent (the pair with highest covisibility weight)
         // Include that children as new parent candidate for the rest

diff --git a/src/Tracking.cc b/src/Tracking.cc
@@ -587,8 +587,14 @@ void Tracking::Run()
                 else{
                     predTcp=imu_proc->propagate(time_frame);
                     assert(imu_proc->getMeasurements().size());
+
+                    Eigen::Matrix<double, 9, 1> velAndBiases = imu_proc->speed_bias_1;
+                    if(experim == DiLiLi)
+                    //Hack: it is observed that imu prediction may be worse than stereo pose differention
+                        velAndBiases.head<3>() = mVelByStereoOdometry;
+
                     ProcessFrame(left_img, right_img, time_frame,
-                                 imu_proc->getMeasurements(), &predTcp, imu_proc->speed_bias_1);
+                                 imu_proc->getMeasurements(), &predTcp, velAndBiases);
                 }
                 if(mState == WORKING){
                     imu_proc->resetStates((imu_.T_imu_from_cam*mpLastFrame->mTcw).inverse(), mpLastFrame->speed_bias);
@@ -1344,8 +1350,6 @@ void  Tracking::ProcessFrameQCV(cv::Mat &im, cv::Mat &right_img, double timeStam
     }
     SLAM_START_TIMER("create_frame");
 
-    // it is observed that imu prediction may be worse than stereo pose differentiation.
-    sb.head<3>() = mVelByStereoOdometry;
     double lastFrameTime = mpLastFrame==NULL? -1:mpLastFrame->mTimeStamp;
     //compute ORB descriptors of vStereoMatches
     mpCurrentFrame=new Frame(im, timeStampSec, mStereoSFM.getNumDenseFeatures(),
@@ -1680,8 +1684,6 @@ void  Tracking::ProcessFrame(cv::Mat &im, cv::Mat &right_img, double timeStampSe
     }
     SLAM_START_TIMER("create_frame");
 
-    // it is observed that imu prediction may be worse than stereo pose differentiation.
-    sb.head<3>() = mVelByStereoOdometry;
     double lastFrameTime = mpLastFrame==NULL? -1:mpLastFrame->mTimeStamp;
     //compute ORB descriptors of vStereoMatches
     mpCurrentFrame=new Frame(im, timeStampSec, mVisoStereo.matcher->getNumDenseFeatures(true),