search

jingpang / LearnVIORB

Other
818 stars 453 forks source link

Code is always on tryInitVIO mode #47

Open ghost opened 6 years ago

ghost commented 6 years ago

I run the code on EuRoC MH_01_easy dataset, and got a final scale of value: 5.899714. Is it the true scale for the MH_01_easy dataset ? Then I wanted to track the program flow with some cout statements on main methods and it seems like the code never goes out of the initialization state to trackingwithimu stage. Here is the initial and final output:

tart processing sequence ... Images in the sequence: 3682

Load Image: 0 Done aggregating imu with n points: 2 Inside Tracking::TrackMonoVI() Inside Tracking::GrabImageMonoVI Inside Frame Constructor Frame id: 0 Assigning features to grid Inside Tracking::Track() Inside Tracking::MonocularInitialization clering mvIMUSinceLastKF and setting referenc frame Inside Frame::GetNavState Inside Frame::GetNavState Inside Initializer::Initializer Load Image: 1 Done aggregating imu with n points: 10 Inside Tracking::TrackMonoVI() Inside Tracking::GrabImageMonoVI Inside Frame Constructor Assigning features to grid Inside Tracking::Track() Inside Tracking::MonocularInitialization Trying to initialize Initialize with nmatches: 401 Inside Initializer::Initialize Load Image: 2 Inside Tracking::TrackMonoVI() Inside Tracking::GrabImageMonoVI Inside Frame Constructor Frame id: 13 Assigning features to grid Inside Tracking::Track() mState: 1 Inside Tracking::MonocularInitialization Trying to initialize Initialize with nmatches: 282 Inside Initializer::Initialize Inside Tracking::CreateInitialMapMonocular Inside KeyFrame Constructor Inside KeyFrame Constructor dt = 0.000, this vs next time: 1403636579.864 vs 1403636579.864 New Map created with 239 points Inside Frame::GetNavState Load Image: 3 Inside Tracking::TrackMonoVI() Inside Tracking::GrabImageMonoVI Inside Frame Constructor init done opengl support available Assigning features to grid Inside Tracking::Track() System initialized Inside !mbOnlyTracking Inside mState=ok and TRACK_WITH_IMU Inside mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2 Inside Tracking::TrackReferenceKeyFrame Inside Optimizer::PoseOptimization(Frame pFrame) bOK at 7E: 1 Local Map tracking Inside Tracking::TrackLocalMap Inside Optimizer::PoseOptimization(Frame pFrame) bOK at 2E: 1 Inside Frame::GetNavState Load Image: 4 Trying to init VIO Inside LocalMapping::TryInitVIO Keyframes: 2 mnLocalWindowSize: 10 Inside Tracking::TrackMonoVI() Inside Tracking::GrabImageMonoVI Inside Frame Constructor Assigning features to grid Inside Tracking::Track() mState: 2 System initialized Inside !mbOnlyTracking Inside mState=ok and TRACK_WITH_IMU !(Inside mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2) Inside Tracking::TrackWithMotionModel Inlier: 195, vDepth: 195, vFeat: 195, matches: 195 This frame has matches: 157 Inside Optimizer::PoseOptimization(Frame pFrame) TrackWithMotionModel nmatchesMap : 124 bOK at 6E: 1 Local Map tracking Inside Tracking::TrackLocalMap Inside Optimizer::PoseOptimization(Frame pFrame) bOK at 2E: 1 Inside KeyFrame Constructor Inside Frame::GetNavState Inside KeyFrame::SetInitialNavStateAndBias dt = 0.000, this vs next time: 1403636579.964 vs 1403636579.964 Inside Frame::GetNavState ...... ........ ....... Load Image: 3339 Inside Tracking::TrackMonoVI() Inside Tracking::GrabImageMonoVI Inside Frame Constructor Assigning features to grid Inside Tracking::Track() mState: 2 System initialized Inside !mbOnlyTracking Inside mState=ok and TRACK_WITH_IMU !(Inside mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2) Inside Tracking::TrackWithMotionModel Inlier: 382, vDepth: 382, vFeat: 382, matches: 382 This frame has matches: 252 Inside Optimizer::PoseOptimization(Frame *pFrame) TrackWithMotionModel nmatchesMap : 197 bOK at 6E: 1 Local Map tracking Inside Tracking::TrackLocalMap dist = 0.963451 Trying to init VIO Inside LocalMapping::TryInitVIO dt = 0.000, this vs next time: 1403636580.264 vs 1403636580.264 dt = 0.000, this vs next time: 1403636580.714 vs 1403636580.714 dt = 0.000, this vs next time: 1403636581.164 vs 1403636581.164

So even after loading 3339 images, the system is still in the trying to init vio mode. But the gw value seems to be 9.7 and it seems to be fine. Also I was able to get the gyro and acc bias. Does it mean that the code doesnot have termination criteria for the IMU initialiazation. Thnaks for the reply. Regards, Dharma KC

imrasp commented 6 years ago

@dharma-kc 5.899714 is normal. I have run this project on EuRoC MH_01_easy dataset and got scale factor around 1 to 6. the trajectory result compare to the ground truth from the dataset show that we have got error around 4-7 cm as result below.

compared 168 absolute pose pairs calculating APE for translation part pose relation... APE w.r.t. translation part (m)

   max  0.144626
  mean  0.067789
median  0.074708
   min  0.000399
  rmse  0.076236
   sse  0.976393
   std  0.034878

result of tested 4 times on MH_01_easy dataset Time: 15.4, sstar: 1.13853, s: 1.14564 -> rmse 0.076236 Time: 15.2, sstar: 4.59188, s: 4.60311 -> rmse 0.060084 Time: 15.4, sstar: 5.18981, s: 5.21842 -> rmse 0.047713 Time: 15.15, sstar: 6.36775, s: 6.40302 -> rmse 0.070640

Hope this answer some of your question.