success mono mode

vio_hw/TODO.md

@@ -1 +1,3 @@
-- 为刚开始的打印参数部分加上更好看的配置，例如条条之类的或者表情
+- 为刚开始的打印参数部分加上更好看的配置，例如条条之类的或者表情
+- 在单目模式下，若不通过后端优化进行过滤，会在关键帧比较多且视差很小的情况下轨迹突然出现尺度问题，该问题可通过调整关键帧判断条件或者开启后端优化来解决
+- 上述问题产生的原因可能是在平移不足时，三角化的点的误差太大了，但是确实满足位姿约束的点

vio_hw/internal/estimator.hpp

@@ -9,6 +9,7 @@
 #include "vio_hw/internal/map_manager.hpp"
 #include "vio_hw/internal/optimization.hpp"
 #include "vio_hw/internal/setting.hpp"
+#include "vio_hw/internal/system_status.hpp"
 
 namespace viohw {
 class Estimator
@@ -18,7 +19,7 @@ class Estimator
   // constructor
   Estimator() = default;
 
-  Estimator( SettingPtr param, MapManagerPtr map_manager, OptimizationPtr optimization );
+  Estimator( SettingPtr param, MapManagerPtr map_manager, OptimizationPtr optimization, SystemStatePtr state );
 
   // backend estimator thread
   void run();
@@ -33,6 +34,7 @@ class Estimator
   SettingPtr param_;
   MapManagerPtr map_manager_;
   OptimizationPtr optimization_;
+  SystemStatePtr system_state_;
 
   bool is_newkf_available_ = false;
   bool is_loose_ba_on_ = false;

vio_hw/internal/setting.hpp

@@ -53,6 +53,7 @@ class Setting
     float klt_max_fb_dist_;
     float klt_err_;
     bool use_brief_;
+    double init_parallax_;
     FeatureBase::FeatureType feature_type_;
     std::string feature_name_;
     TrackerBase::TrackerType tracker_type_;
@@ -223,6 +224,7 @@ inline std::ostream& operator<<(
   os << std::boolalpha;
   os << std::right << std::setw(20) << "Feature.Type: "    << std::left << std::setw(10) <<  setting.feature_type_ << std::endl
      << std::right << std::setw(20) << "Feature.name: "    << std::left << std::setw(10) <<  setting.feature_name_ << std::endl
+     << std::right << std::setw(20) << "Init.Parallax: "    << std::left << std::setw(10) <<  setting.init_parallax_ << std::endl
      << std::right << std::setw(20) << "Tracker.Type: "    << std::left << std::setw(10) <<  setting.tracker_type_ << std::endl
      << std::right << std::setw(20) << "Tracker.name: "    << std::left << std::setw(10) <<  setting.tracker_name_ << std::endl
      << std::right << std::setw(20) << "Max.Feature.Num: " << std::left << std::setw(10) <<  setting.max_feature_num_ << std::endl

vio_hw/internal/system_status.hpp

@@ -0,0 +1,60 @@
+#pragma once
+#include <atomic>
+#include <mutex>
+
+namespace viohw {
+
+enum TrackingStatus
+{
+  TrackLost,
+  TrackBad,
+  TrackGood
+};
+
+enum InitStatus
+{
+  NotInit,
+  InitSuccess,
+  InitFailed
+};
+
+class SystemState
+{
+public:
+  SystemState() = default;
+  ~SystemState() = default;
+
+  TrackingStatus getTrackerStatus() const {
+    std::lock_guard<std::mutex> lck( tracker_status_mutex_ );
+    return tracking_status_;
+  }
+
+  void setTrackerStatus( const TrackingStatus &status ) {
+    std::lock_guard<std::mutex> lck( tracker_status_mutex_ );
+    tracking_status_ = status;
+  }
+
+  InitStatus getInitStatus() const {
+    std::lock_guard<std::mutex> lck( init_status_mutex_ );
+    return init_status_;
+  }
+
+  void setInitStatus( const InitStatus &status ) {
+    std::lock_guard<std::mutex> lck( init_status_mutex_ );
+    init_status_ = status;
+  }
+
+  std::atomic<bool> is_local_ba_{ false };
+  std::atomic<bool> is_request_reset_{ false };
+
+private:
+  mutable std::mutex tracker_status_mutex_;
+  mutable std::mutex init_status_mutex_;
+  TrackingStatus tracking_status_ = TrackGood;
+  InitStatus init_status_ = NotInit;
+};
+
+typedef std::shared_ptr<SystemState> SystemStatePtr;
+typedef std::shared_ptr<const SystemState> SystemStateConstPtr;
+
+}  // namespace viohw

vio_hw/internal/visual_frontend.hpp

@@ -1,18 +1,20 @@
 #ifndef VIO_HELLO_WORLD_VISUAL_FRONTEND_HPP
 #define VIO_HELLO_WORLD_VISUAL_FRONTEND_HPP
 
+#include "geometry/motion_ba/motion_ba.hpp"
 #include "vio_hw/internal/constant_motion_model.hpp"
 #include "vio_hw/internal/frame.hpp"
 #include "vio_hw/internal/map_manager.hpp"
 #include "vio_hw/internal/setting.hpp"
+#include "vio_hw/internal/system_status.hpp"
 #include "vio_hw/internal/tracker/tracker_base.hpp"
 #include "vio_hw/internal/viz/visualization_base.hpp"
-#include "geometry/motion_ba/motion_ba.hpp"
+#include "geometry/epipolar/epipolar_constraint.hpp"
 
 namespace viohw {
 class VisualFrontEnd
 {
- public:
+public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   // default construction function
@@ -22,17 +24,19 @@ class VisualFrontEnd
   ~VisualFrontEnd() = default;
 
   // construction function
-  VisualFrontEnd(SettingPtr setting, FramePtr frame, MapManagerPtr manager,
-                 TrackerBasePtr tracker, VisualizationBasePtr viz);
+  VisualFrontEnd( SettingPtr setting, FramePtr frame, MapManagerPtr manager, TrackerBasePtr tracker,
+                  VisualizationBasePtr viz, SystemStatePtr state );
 
   // tracking left image
-  bool TrackerMono(cv::Mat &image, double time);
+  bool TrackerMono( cv::Mat &image, double time );
+
+  bool VisualMonoInit();
 
   // tracking image and build keyframe
-  bool VisualTracking(cv::Mat &image, double time);
+  bool VisualTracking( cv::Mat &image, double time );
 
   // preprocess image, build pyramid for tracking and apply clahe
-  void PreProcessImage(cv::Mat &image);
+  void PreProcessImage( cv::Mat &image );
 
   // optical flow tracking for frame before and after
   void KLTTracking();
@@ -57,14 +61,15 @@ class VisualFrontEnd
   // get current frame[left image] pyramid image
   std::vector<cv::Mat> GetCurrentFramePyramid() const;
 
-  float ComputeParallax(const int kfid, bool do_unrot, bool median, bool is_2donly);
+  float ComputeParallax( const int kfid, bool do_unrot, bool median, bool is_2donly );
 
- private:
+private:
   MapManagerPtr map_manager_;
   FramePtr current_frame_;
   SettingPtr param_;
   TrackerBasePtr tracker_;
   VisualizationBasePtr viz_;
+  SystemStatePtr system_state_;
 
   ConstantMotionModel motion_model_;
 

vio_hw/internal/world_manager.hpp

@@ -13,6 +13,7 @@
 #include "vio_hw/internal/map_manager.hpp"
 #include "vio_hw/internal/mapping.hpp"
 #include "vio_hw/internal/setting.hpp"
+#include "vio_hw/internal/system_status.hpp"
 #include "vio_hw/internal/tracker/tracker_base.hpp"
 #include "vio_hw/internal/visual_frontend.hpp"
 #include "vio_hw/internal/viz/visualization_base.hpp"
@@ -25,6 +26,7 @@ class WorldManager
   void run();
   const std::shared_ptr<Setting> getParams() const { return params_; }
   void addNewStereoImages( double time, cv::Mat &im0, cv::Mat &im1 );
+  void addNewMonoImage( const double time, cv::Mat &im0 );
   bool getNewImage( cv::Mat &iml, cv::Mat &imr, double &time );
   void setupCalibration();
   bool VisualizationImage();
@@ -64,6 +66,9 @@ class WorldManager
   // imu database
   backend::IMU::IMUDataBasePtr imu_database_;
   bool is_init_imu_pose_ = false;
+
+  // system state
+  SystemStatePtr system_state_;
 };
 
 typedef std::chrono::time_point<std::chrono::high_resolution_clock> TimeStamp;

vio_hw/node/hello_world_vio.cpp

@@ -35,10 +35,12 @@ class SensorManager
 
     sub_left_img_ = nh_.subscribe( slam_world_->getParams()->cam_setting_.topic_left_right_[0], 1,
                                    &SensorManager::subLeftImage, this );
-    sub_right_img_ = nh_.subscribe( slam_world_->getParams()->cam_setting_.topic_left_right_[1], 1,
-                                    &SensorManager::subRightImage, this );
-    sub_imu_ = nh_.subscribe( slam_world_->getParams()->imu_setting_.imu_topic_, 10,
-                              &SensorManager::subIMU, this );
+    if ( slam_world_->getParams()->slam_setting_.stereo_mode_ )
+      sub_right_img_ = nh_.subscribe( slam_world_->getParams()->cam_setting_.topic_left_right_[1],
+                                      1, &SensorManager::subRightImage, this );
+    if ( slam_world_->getParams()->slam_setting_.use_imu_ )
+      sub_imu_ = nh_.subscribe( slam_world_->getParams()->imu_setting_.imu_topic_, 10,
+                                &SensorManager::subIMU, this );
   };
 
   ~SensorManager() = default;
@@ -104,6 +106,19 @@ class SensorManager
             }
           }
         }
+      } else {
+        cv::Mat image0;
+        std::lock_guard<std::mutex> lock( img_mutex_ );
+
+        if ( !img0_buf_.empty() ) {
+          double time = img0_buf_.front()->header.stamp.toSec();
+          image0 = getGrayImageFromMsg( img0_buf_.front() );
+          img0_buf_.pop();
+
+          if ( !image0.empty() ) {
+            slam_world_->addNewMonoImage( time, image0 );
+          }
+        }
       }
       std::chrono::milliseconds dura( 1 );
       std::this_thread::sleep_for( dura );

vio_hw/params/euroc/euroc_stereo_imu.yaml

@@ -29,7 +29,8 @@ FeatureAndTracker:
   # common params
   max.kps.num: 300
   max.kps.distance: 20
-  use.clahe: 0
+  init.parallax: 20.
+  use.clahe: 1
   clahe.val: 3
   track.keyframetoframe: 0
   use.brief: 1
@@ -63,11 +64,11 @@ SLAM:
 
 LoopCloser:
   use.loop: 0
-  loop.threshold: 0.1
+  loop.threshold: 0.07
 
 Backend:
   use.backend: 1
-  opt.window.size: 25
+  opt.window.size: 15
 
 
 # Camera Extrinsic parameters T_b_ci ( v_b = T_b_ci * v_ci )

vio_hw/params/zed/zed2i.yaml

@@ -14,20 +14,22 @@ Camera:
   right.K: [267.82012939453125, 267.82012939453125, 320.6171569824219, 188.72052001953125]
   left.distortion.coefficient: [0.0, 0.0, 0.0, 0.0]
   right.distortion.coefficient: [0.0, 0.0, 0.0, 0.0]
+  FPS: 30
 
 ConfigFilePath:
   vocabulary.path: "/home/weihao/workspace/vio-hello-world-impl/thirdparty/Vocabulary/ORBvoc.txt"
-  kf.traj.save.path: ""
+  kf.traj.save.path: "/home/weihao/workspace/vio-hello-world-impl/vio_hw/output/euroc_out/MH_05_KF.txt"
+  dfm.config.file.path: "/home/weihao/workspace/vio-hello-world-impl/dfm/params/splg_config.yaml"
   bag.file.path: ""
 
 
 FeatureAndTracker:
-  feature.Type: 0
-  tracker.Type: 0
+  feature.Type: "HARRIS"  # ["ORB_CV","HARRIS","ORB","SUPER_POINT"]
+  tracker.Type: "OPTICAL_FLOW" # ["OPTICAL_FLOW", "LIGHT_GLUE"]
   # common params
-  max.kps.num: 350
-  max.kps.distance: 20
-  use.clahe: 1
+  max.kps.num: 500
+  max.kps.distance: 15
+  use.clahe: 0
   clahe.val: 3
   track.keyframetoframe: 0
   use.brief: 1
@@ -49,6 +51,8 @@ IMU:
   acc_w: 0.2
   gyr_n: 0.3
   gyr_w: 0.4
+  g_norm: 9.81
+  FPS: 200
 
 SLAM:
   stereo.mode: 1
@@ -58,12 +62,12 @@ SLAM:
   use.Pangolin: 0
 
 LoopCloser:
-  use.loop: 0
-  loop.threshold: 0.6
+  use.loop: 1
+  loop.threshold: 0.07
 
 Backend:
   use.backend: 1
-  opt.window.size: 15
+  opt.window.size: 10
 
 
 

vio_hw/src/estimator.cpp

@@ -2,8 +2,12 @@
 
 namespace viohw {
 
-Estimator::Estimator( SettingPtr param, MapManagerPtr map_manager, OptimizationPtr optimization )
-    : param_( param ), map_manager_( map_manager ), optimization_( optimization ) {}
+Estimator::Estimator( SettingPtr param, MapManagerPtr map_manager, OptimizationPtr optimization,
+                      SystemStatePtr state )
+    : param_( param ),
+      map_manager_( map_manager ),
+      optimization_( optimization ),
+      system_state_( state ) {}
 
 void Estimator::run() {
   bool open_backend = param_->backend_optimization_setting_.open_backend_opt_;
@@ -23,7 +27,11 @@ void Estimator::run() {
 }
 
 void Estimator::ApplyLocalBA() {
-  int mincstkfs = 2;
+  int mincstkfs = 1;
+
+  if ( !param_->slam_setting_.stereo_mode_ ) {
+    mincstkfs = 2;
+  }
 
   if ( newkf_->kfid_ < mincstkfs ) {
     return;
@@ -34,15 +42,13 @@ void Estimator::ApplyLocalBA() {
 
   std::lock_guard<std::mutex> lock2( map_manager_->optim_mutex_ );
 
-  // TODO
   // We signal that Estimator is performing BA
-  // pslamstate_->blocalba_is_on_ = true;
+  system_state_->is_local_ba_.store( true );
 
-  bool use_robust_cost = true;
-  optimization_->LocalBA( *newkf_, use_robust_cost );
+  optimization_->LocalBA( *newkf_, true );
 
   // We signal that Estimator is stopping BA
-  // pslamstate_->blocalba_is_on_ = false;
+  system_state_->is_local_ba_.store( false );
 }
 
 bool Estimator::GetNewKf() {

vio_hw/src/mapping.cpp

@@ -85,10 +85,6 @@ void Mapping::TriangulateTemporal( Frame& frame ) {
   }
 
   std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > vleftbvs, vrightbvs;
-  // Init a pkf object that will point to the prev KF to use for triangulation
-  std::shared_ptr<Frame> pkf;
-  pkf.reset( new Frame() );
-  pkf->kfid_ = -1;
 
   // Relative motions between new KF and prev. KFs
   int relkfid = -1;
@@ -123,7 +119,7 @@ void Mapping::TriangulateTemporal( Frame& frame ) {
     }
 
     // Get the 1st KF observation of the related MP
-    pkf = map_manager_->GetKeyframe( kfid );
+    auto pkf = map_manager_->GetKeyframe( kfid );
 
     if ( pkf == nullptr ) {
       continue;
@@ -142,6 +138,14 @@ void Mapping::TriangulateTemporal( Frame& frame ) {
     if ( params_->slam_setting_.stereo_mode_ && Tcicj.translation().norm() < 0.01 ) {
       continue;
     }
+
+    // TODO if using imu in mono mode, the [Tcicj.translation().norm() < 0.01] also can be use
+    // If no motion between both KF, skip
+    // if ( !params_->slam_setting_.stereo_mode_ && params_->slam_setting_.use_imu_ &&
+    //      Tcicj.translation().norm() < 0.01 ) {
+    //   continue;
+    // }
+
     // Get obs kp
     Keypoint kfkp = pkf->GetKeypointById( vkps.at( i ).lmid_ );
     if ( kfkp.lmid_ != vkps.at( i ).lmid_ ) {