From 235955950db87d88b3ea9630554bbc3f002df347 Mon Sep 17 00:00:00 2001
From: weihaoysgs <weihao_ysgs@163.com>
Date: Tue, 16 Apr 2024 22:15:07 +0800
Subject: [PATCH] success mono mode

---
 vio_hw/TODO.md                            |  4 +-
 vio_hw/internal/estimator.hpp             |  4 +-
 vio_hw/internal/setting.hpp               |  2 +
 vio_hw/internal/system_status.hpp         | 60 ++++++++++++++++
 vio_hw/internal/visual_frontend.hpp       | 23 +++---
 vio_hw/internal/world_manager.hpp         |  5 ++
 vio_hw/node/hello_world_vio.cpp           | 23 ++++--
 vio_hw/params/euroc/euroc_stereo_imu.yaml |  7 +-
 vio_hw/params/zed/zed2i.yaml              | 22 +++---
 vio_hw/src/estimator.cpp                  | 22 +++---
 vio_hw/src/mapping.cpp                    | 14 ++--
 vio_hw/src/optimization.cpp               | 16 ++---
 vio_hw/src/setting.cpp                    |  1 +
 vio_hw/src/visual_frontend.cpp            | 88 ++++++++++++++++++++++-
 vio_hw/src/world_manager.cpp              | 21 ++++--
 15 files changed, 256 insertions(+), 56 deletions(-)
 create mode 100644 vio_hw/internal/system_status.hpp

diff --git a/vio_hw/TODO.md b/vio_hw/TODO.md
index ca5baad..2f51166 100644
--- a/vio_hw/TODO.md
+++ b/vio_hw/TODO.md
@@ -1 +1,3 @@
-- 为刚开始的打印参数部分加上更好看的配置，例如条条之类的或者表情
\ No newline at end of file
+- 为刚开始的打印参数部分加上更好看的配置，例如条条之类的或者表情
+- 在单目模式下，若不通过后端优化进行过滤，会在关键帧比较多且视差很小的情况下轨迹突然出现尺度问题，该问题可通过调整关键帧判断条件或者开启后端优化来解决
+- 上述问题产生的原因可能是在平移不足时，三角化的点的误差太大了，但是确实满足位姿约束的点
\ No newline at end of file
diff --git a/vio_hw/internal/estimator.hpp b/vio_hw/internal/estimator.hpp
index 54cace6..974b5d9 100644
--- a/vio_hw/internal/estimator.hpp
+++ b/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;
diff --git a/vio_hw/internal/setting.hpp b/vio_hw/internal/setting.hpp
index a733649..3fb4de9 100644
--- a/vio_hw/internal/setting.hpp
+++ b/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
diff --git a/vio_hw/internal/system_status.hpp b/vio_hw/internal/system_status.hpp
new file mode 100644
index 0000000..467f0f3
--- /dev/null
+++ b/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
diff --git a/vio_hw/internal/visual_frontend.hpp b/vio_hw/internal/visual_frontend.hpp
index 68b61f5..2525642 100644
--- a/vio_hw/internal/visual_frontend.hpp
+++ b/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_;
 
diff --git a/vio_hw/internal/world_manager.hpp b/vio_hw/internal/world_manager.hpp
index e3b31d4..ae9ab89 100644
--- a/vio_hw/internal/world_manager.hpp
+++ b/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;
diff --git a/vio_hw/node/hello_world_vio.cpp b/vio_hw/node/hello_world_vio.cpp
index 4118541..4917fe3 100644
--- a/vio_hw/node/hello_world_vio.cpp
+++ b/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 );
diff --git a/vio_hw/params/euroc/euroc_stereo_imu.yaml b/vio_hw/params/euroc/euroc_stereo_imu.yaml
index aac56a4..b7562f7 100644
--- a/vio_hw/params/euroc/euroc_stereo_imu.yaml
+++ b/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 )
diff --git a/vio_hw/params/zed/zed2i.yaml b/vio_hw/params/zed/zed2i.yaml
index 509ac89..f588e0e 100644
--- a/vio_hw/params/zed/zed2i.yaml
+++ b/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
 
 
 
diff --git a/vio_hw/src/estimator.cpp b/vio_hw/src/estimator.cpp
index 48b3253..966b6d5 100644
--- a/vio_hw/src/estimator.cpp
+++ b/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() {
diff --git a/vio_hw/src/mapping.cpp b/vio_hw/src/mapping.cpp
index ccd8948..6d91f2f 100644
--- a/vio_hw/src/mapping.cpp
+++ b/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_ ) {
diff --git a/vio_hw/src/optimization.cpp b/vio_hw/src/optimization.cpp
index 7420955..8fb1690 100644
--- a/vio_hw/src/optimization.cpp
+++ b/vio_hw/src/optimization.cpp
@@ -92,7 +92,7 @@ bool Optimization::LocalPoseGraph( Frame &new_frame, int kf_loop_id, const Sophu
   std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> vwpt;
   std::vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> vTwc;
 
-  std::lock_guard<std::mutex> lock(map_manager_->map_mutex_);
+  std::lock_guard<std::mutex> lock( map_manager_->map_mutex_ );
   // get updated KFs / MPs
   for ( const auto &kf_id_pkf : map_kfs ) {
     int kf_id = kf_id_pkf.first;
@@ -284,12 +284,6 @@ void Optimization::LocalBA( Frame &newframe, const bool buse_robust_cost ) {
     problem.SetParameterBlockConstant( rlextrinpose.values() );
   }
 
-  // Get the new KF covisible KFs
-  // std::map<int,int> map_covkfs = newframe.getCovisibleKfMap();
-
-  // Add the new KF to the map with max score
-  // map_covkfs.emplace(newframe.kfid_, newframe.nb3dkps_);
-
   // Keep track of MPs no suited for BA for speed-up
   std::unordered_set<int> set_badlmids;
 
@@ -299,7 +293,7 @@ void Optimization::LocalBA( Frame &newframe, const bool buse_robust_cost ) {
 
   for ( int i = 0; i < inertial_frames.size(); i++ ) {
     std::shared_ptr<Frame> pkf = inertial_frames[i];
-    // Sophus::SE3d Tcb = pslamstate_->T_imu_camera_.inverse();
+
     map_id_posespar_.emplace( pkf->kfid_, PoseParametersBlock( pkf->kfid_, pkf->GetTwc() ) );
     tceres::LocalParameterization *local_parameterization = new SE3LeftParameterization();
     problem.AddParameterBlock( map_id_posespar_.at( pkf->kfid_ ).values(), 7,
@@ -313,9 +307,15 @@ void Optimization::LocalBA( Frame &newframe, const bool buse_robust_cost ) {
     }
     map_local_pkfs.emplace( pkf->kfid_, pkf );
   }
+
+  // set const frame
   {
     std::shared_ptr<Frame> before_kf = inertial_frames.back();
     problem.SetParameterBlockConstant( map_id_posespar_.at( before_kf->kfid_ ).values() );
+    if ( nmincstkfs > 1 ) {
+      std::shared_ptr<Frame> kf = inertial_frames.at( inertial_frames.size() - 1 );
+      problem.SetParameterBlockConstant( map_id_posespar_.at( kf->kfid_ ).values() );
+    }
   }
 
   // Go through the MPs to optimize
diff --git a/vio_hw/src/setting.cpp b/vio_hw/src/setting.cpp
index b7751bb..d03c0a4 100644
--- a/vio_hw/src/setting.cpp
+++ b/vio_hw/src/setting.cpp
@@ -68,6 +68,7 @@ void Setting::readFeatureTrackerParams( const cv::FileNode& node ) {
   node["klt.win.size"] >> feat_tracker_setting_.klt_win_size_;
   node["klt.pyr.level"] >> feat_tracker_setting_.klt_pyr_level_;
   node["use.brief"] >> feat_tracker_setting_.use_brief_;
+  node["init.parallax"] >> feat_tracker_setting_.init_parallax_;
 
   node["klt.max.iter"] >> feat_tracker_setting_.klt_max_iter_;
   node["klt.max.px.precision"] >> feat_tracker_setting_.klt_max_px_precision_;
diff --git a/vio_hw/src/visual_frontend.cpp b/vio_hw/src/visual_frontend.cpp
index a2e383f..74776db 100644
--- a/vio_hw/src/visual_frontend.cpp
+++ b/vio_hw/src/visual_frontend.cpp
@@ -4,12 +4,13 @@ namespace viohw {
 
 VisualFrontEnd::VisualFrontEnd( viohw::SettingPtr state, viohw::FramePtr frame,
                                 viohw::MapManagerPtr map, viohw::TrackerBasePtr tracker,
-                                VisualizationBasePtr viz )
+                                VisualizationBasePtr viz, SystemStatePtr system )
     : param_( state ),
       current_frame_( frame ),
       map_manager_( map ),
       tracker_( tracker ),
-      viz_( viz ) {
+      viz_( viz ),
+      system_state_( system ) {
   use_clahe_ = param_->feat_tracker_setting_.use_clahe_;
 
   if ( use_clahe_ ) {
@@ -58,6 +59,22 @@ bool VisualFrontEnd::TrackerMono( cv::Mat& image, double time ) {
   // show tracking result to ui
   ShowTrackingResult();
 
+  if ( !param_->slam_setting_.stereo_mode_ && system_state_->getInitStatus() == NotInit ) {
+    if ( current_frame_->nb2dkps_ < 50 ) {
+      // TODO, reset system && when first stereo matcher too less, we also need reset system
+      system_state_->is_request_reset_.store( true );
+      LOG( WARNING ) << "[Visual-Front-End-Init]: Kps too Less, reset System";
+      return false;
+    } else if ( VisualMonoInit() ) {
+      LOG( INFO ) << "[Visual-Front-End]: Mono Visual SLAM Ready For Initialization!";
+      system_state_->setInitStatus( InitSuccess );
+      return true;
+    } else {
+      LOG( WARNING ) << "[Visual-Front-End-Init]: Not Ready To Init Yet!";
+      return false;
+    }
+  }
+
   // compute current visual frontend pose
   ComputePose();
 
@@ -68,6 +85,71 @@ bool VisualFrontEnd::TrackerMono( cv::Mat& image, double time ) {
   return CheckIsNewKeyframe();
 }
 
+bool VisualFrontEnd::VisualMonoInit() {
+  double avg_rot_parallax = ComputeParallax( current_frame_->kfid_, false, true, false );
+  if ( avg_rot_parallax < param_->feat_tracker_setting_.init_parallax_ ) {
+    LOG( WARNING ) << "Parallax Too Small: " << avg_rot_parallax;
+    return false;
+  } else {
+    // Get prev. KF
+    auto pkf = map_manager_->GetKeyframe( current_frame_->kfid_ );
+    if ( pkf == nullptr ) {
+      return false;
+    }
+
+    // Setup Essential Matrix computation for OpenGV-based filtering
+    std::vector<int> vkpsids, voutliersidx;
+    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > vkfbvs, vcurbvs;
+    // Get bvs and compute the rotation compensated parallax for all cur kps
+    // for( const auto &kp : pcurframe_->getKeypoints() ) {
+    for ( const auto& it : current_frame_->mapkps_ ) {
+      auto& kp = it.second;
+      // Get the prev. KF related kp if it exists
+      auto kfkp = pkf->GetKeypointById( kp.lmid_ );
+      if ( kfkp.lmid_ != kp.lmid_ ) {
+        continue;
+      }
+      // Store the bvs and their ids
+      vkfbvs.push_back( kfkp.bv_ );
+      vcurbvs.push_back( kp.bv_ );
+      vkpsids.push_back( kp.lmid_ );
+    }
+    if ( vkpsids.size() < 8 ) {
+      LOG( WARNING ) << "Not enough kps to compute 5-pt Essential Matrix";
+      return false;
+    }
+
+    Eigen::Matrix3d Rkfc = Eigen::Matrix3d::Identity();
+    Eigen::Vector3d tkfc = Eigen::Vector3d ::Zero();
+
+    bool success = geometry::Opencv5ptEssentialMatrix(
+        vkfbvs, vcurbvs, 100, 3., false, current_frame_->pcalib_leftcam_->fx_,
+        current_frame_->pcalib_leftcam_->fy_, Rkfc, tkfc, voutliersidx );
+
+    if ( !success ) {
+      LOG( WARNING ) << "Not enough inline kps to compute Essential Matrix";
+      return false;
+    }
+
+    // Remove outliers from cur. Frame
+    for ( const auto& idx : voutliersidx ) {
+      // MapManager is responsible for all the removing operations.
+      map_manager_->RemoveObsFromCurFrameById( vkpsids.at( idx ) );
+    }
+
+    // Arbitrary scale
+    tkfc.normalize();
+    tkfc = tkfc.eval() * 0.25;
+
+    LOG( INFO ) << ">>> Essential Mat init : " << tkfc.transpose();
+    Sophus::SE3d T_w_kf = pkf->GetTwc();
+    Sophus::SE3d T_kf_cur( Rkfc, tkfc );
+    current_frame_->SetTwc( T_w_kf * T_kf_cur );
+  }
+
+  return true;
+}
+
 void VisualFrontEnd::PreProcessImage( cv::Mat& img_raw ) {
   if ( use_clahe_ ) {
     clahe_->apply( img_raw, cur_img_ );
@@ -138,7 +220,7 @@ void VisualFrontEnd::KLTTracking() {
 
     size_t good_num = 0;
 
-    feat_cur.conservativeResize(259, feat_prev.cols());
+    feat_cur.conservativeResize( 259, feat_prev.cols() );
     for ( size_t i = 0; i < vkps.size(); i++ ) {
       if ( kpstatus.at( i ) ) {
         current_frame_->UpdateKeypoint( vkp_ids.at( i ), vpriors.at( i ) );
diff --git a/vio_hw/src/world_manager.cpp b/vio_hw/src/world_manager.cpp
index 78c4b8b..2760704 100644
--- a/vio_hw/src/world_manager.cpp
+++ b/vio_hw/src/world_manager.cpp
@@ -14,6 +14,9 @@ WorldManager::WorldManager( std::shared_ptr<Setting>& setting ) : params_( setti
   // create feature extractor
   GenerateFeatureExtractorBase();
 
+  // create feature tracker
+  GenerateFeatureTrackerMatcherBase();
+
   // create IMU database
   imu_database_.reset( new backend::IMU::IMUDataBase( params_->imu_setting_.imu_FPS_,
                                                       params_->cam_setting_.camera_FPS_ ) );
@@ -22,8 +25,7 @@ WorldManager::WorldManager( std::shared_ptr<Setting>& setting ) : params_( setti
   VisualizationBase::VisualizationOption viz_option{ VisualizationBase::RVIZ };
   viz_ = VisualizationBase::Create( viz_option );
 
-  // create feature tracker
-  GenerateFeatureTrackerMatcherBase();
+  system_state_.reset( new SystemState );
 
   // TODO: for [ncellsize] param
   // create current frame
@@ -34,11 +36,12 @@ WorldManager::WorldManager( std::shared_ptr<Setting>& setting ) : params_( setti
   }
 
   // create map manager
-  map_manager_.reset( new MapManager( params_, current_frame_, feature_extractor_, tracker_for_mapping_ ) );
+  map_manager_.reset(
+      new MapManager( params_, current_frame_, feature_extractor_, tracker_for_mapping_ ) );
 
   // create visual frontend
   visual_frontend_.reset(
-      new VisualFrontEnd( params_, current_frame_, map_manager_, tracker_, viz_ ) );
+      new VisualFrontEnd( params_, current_frame_, map_manager_, tracker_, viz_, system_state_ ) );
 
   // create optimization
   optimization_.reset( new Optimization( params_, map_manager_ ) );
@@ -47,7 +50,7 @@ WorldManager::WorldManager( std::shared_ptr<Setting>& setting ) : params_( setti
   loop_closer_.reset( new LoopCloser( params_, map_manager_, optimization_ ) );
 
   // create estimator thread
-  estimator_.reset( new Estimator( params_, map_manager_, optimization_ ) );
+  estimator_.reset( new Estimator( params_, map_manager_, optimization_, system_state_ ) );
 
   // create mapping thread, and mapping will create sub thread for Estimator and LoopClosing
   mapping_.reset( new Mapping( params_, map_manager_, current_frame_, loop_closer_, estimator_ ) );
@@ -198,6 +201,14 @@ bool WorldManager::GenerateFeatureExtractorBase() {
   return true;
 }
 
+void WorldManager::addNewMonoImage( const double time, cv::Mat& im0 ) {
+  std::lock_guard<std::mutex> lock( img_mutex_ );
+  img_left_queen_.push( im0 );
+  img_time_queen_.push( time );
+
+  is_new_img_available_ = true;
+}
+
 void WorldManager::addNewStereoImages( const double time, cv::Mat& im0, cv::Mat& im1 ) {
   std::lock_guard<std::mutex> lock( img_mutex_ );
   img_left_queen_.push( im0 );