success tracker

common/print_tools.hpp

@@ -63,7 +63,7 @@ struct dependent_false : std::false_type
 };
 
 inline void printHelloWorldVIO() {
-  std::cout << BLUE
+  std::cout << BLUE  << std::endl
             << "  _   _      _ _        __        __         _     _  __     _____ ___  \n"
                " | | | | ___| | | ___   \\ \\      / ___  _ __| | __| | \\ \\   / |_ _/ _ \\ \n"
                " | |_| |/ _ | | |/ _ \\   \\ \\ /\\ / / _ \\| '__| |/ _` |  \\ \\ / / | | | | |\n"

vio_hw/CMakeLists.txt

@@ -53,4 +53,9 @@ add_executable(hello_world_vio node/hello_world_vio.cpp)
 add_backward(hello_world_vio)
 target_link_libraries(hello_world_vio
         vio_hw
+)
+add_executable(run_kitti_node node/run_kitti_node.cpp)
+add_backward(run_kitti_node)
+target_link_libraries(run_kitti_node
+        vio_hw
 )

vio_hw/internal/camera_calibration.hpp

@@ -36,6 +36,7 @@
 #include <opencv2/core/eigen.hpp>
 #include <opencv2/imgproc.hpp>
 #include <sophus/se3.hpp>
+#include <glog/logging.h>
 #include <string>
 namespace viohw {
 

vio_hw/internal/frame.hpp

@@ -32,6 +32,7 @@ class Frame
   void ComputeKeypoint(const cv::Point2f &pt, Keypoint &kp);
   void UpdateKeypoint(const int lmid, const cv::Point2f &pt);
   void RemoveKeypointById(const int lmid);
+  Keypoint GetKeypointById(const int lmid) const;
   // For using frame in ordered containers
   bool operator<(const Frame &f) const { return id_ < f.id_; }
 

vio_hw/internal/map_manager.hpp

@@ -29,7 +29,8 @@ class MapManager
   void AddKeypointsToFrame(const std::vector<cv::Point2f> &vpts, const std::vector<cv::Mat> &vdescs,
                            Frame &frame);
   void RemoveObsFromCurFrameById(const int lmid);
-
+  std::shared_ptr<Frame> GetKeyframe(const int kfid) const;
+  std::shared_ptr<MapPoint> GetMapPoint(const int lmid) const;
  private:
   int lm_id_, kf_id_;
   int num_lms_, num_kfs_;

vio_hw/internal/visual_frontend.hpp

@@ -6,6 +6,7 @@
 #include "vio_hw/internal/map_manager.hpp"
 #include "vio_hw/internal/setting.hpp"
 #include "vio_hw/internal/tracker/tracker_base.hpp"
+#include "vio_hw/internal/viz/visualization_base.hpp"
 
 namespace viohw {
 class VisualFrontEnd
@@ -21,7 +22,7 @@ class VisualFrontEnd
 
   // construction function
   VisualFrontEnd(SettingPtr setting, FramePtr frame, MapManagerPtr manager,
-                 TrackerBasePtr tracker);
+                 TrackerBasePtr tracker, VisualizationBasePtr viz);
 
   // tracking left image
   bool TrackerMono(cv::Mat &image, double time);
@@ -35,11 +36,27 @@ class VisualFrontEnd
   // optical flow tracking for frame before and after
   void KLTTracking();
 
+  // filter outlier via epipolar constraint
+  void Epipolar2d2dFiltering();
+
+  // draw tracker result and show in ui
+  void ShowTrackingResult();
+
+  // check current frame is keyframe
+  bool CheckIsNewKeyframe();
+
+  // compute visual frontend pose
+  void ComputePose();
+
+  // update motion model, IMU or Constant model
+  void UpdateMotionModel();
+
  private:
   MapManagerPtr map_manager_;
   FramePtr current_frame_;
   SettingPtr param_;
   TrackerBasePtr tracker_;
+  VisualizationBasePtr viz_;
 
   ConstantMotionModel motion_model_;
 

vio_hw/internal/viz/rviz_visualization.hpp

@@ -2,13 +2,22 @@
 #define VIO_HELLO_WORLD_RVIZ_VISUALIZATION_HPP
 
 #include "vio_hw/internal/viz/visualization_base.hpp"
+#include "vio_hw/internal/viz/camera_visualizer.hpp"
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CompressedImage.h>
+#include <sensor_msgs/PointCloud.h>
+#include <nav_msgs/Path.h>
+#include <nav_msgs/Odometry.h>
+#include <glog/logging.h>
+#include <cv_bridge/cv_bridge.h>
 
 namespace viohw {
 
 class RvizVisualization : public VisualizationBase
 {
  public:
-  RvizVisualization() = default;
+  explicit RvizVisualization();
 
   bool showTrackerResultImage(const cv::Mat &img) override;
 
@@ -20,6 +29,11 @@ class RvizVisualization : public VisualizationBase
 
   bool addPoint(const Eigen::Vector3d &t,
                 const Eigen::Vector3d &color) override;
+
+ private:
+  CameraPoseVisualization camera_pose_visual_;
+  ros::Publisher pub_kfs_traj_, pub_vo_traj_;
+  ros::Publisher pub_img_tracker_result_;
 };
 
 }  // namespace viohw

vio_hw/internal/viz/visualization_base.hpp

@@ -39,6 +39,9 @@ class VisualizationBase
 
 };
 
+typedef std::shared_ptr<VisualizationBase> VisualizationBasePtr;
+typedef std::shared_ptr<const VisualizationBase> VisualizationBaseConstPtr;
+
 }  // namespace viohw
 
 #endif  // VIO_HELLO_WORLD_VISUALIZATION_BASE_HPP

vio_hw/node/hello_world_vio.cpp

@@ -22,7 +22,7 @@
 #include "tceres/solver.h"
 #include "vio_hw/internal/world_manager.hpp"
 
-DEFINE_string(config_file_path, "../vio_hw/params/euroc_stereo_imu.yaml",
+DEFINE_string(config_file_path, "../vio_hw/params/euroc/euroc_stereo_imu.yaml",
               "config file path");
 
 class SensorManager

vio_hw/node/run_kitti_node.cpp

@@ -0,0 +1,127 @@
+// Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+// Everyone is permitted to copy and distribute verbatim copies
+// of this license document, but changing it is not allowed.
+// This file is part of vio-hello-world Copyright (C) 2023 ZJU
+// You should have received a copy of the GNU General Public License
+// along with vio-hello-world. If not, see <https://www.gnu.org/licenses/>.
+// Author: weihao([email protected]), M.S at Zhejiang University
+
+#include <cv_bridge/cv_bridge.h>
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <thread>
+
+#include "feat/harris/harris.h"
+#include "tceres/problem.h"
+#include "tceres/solver.h"
+#include "vio_hw/internal/world_manager.hpp"
+
+DEFINE_string(config_file_path, "../vio_hw/params/kitti/kitti_00-02.yaml", "config file path");
+DEFINE_string(kitti_dataset_path,
+              "/home/weihao/dataset/kitti/data_odometry_gray/dataset/sequences/00",
+              "kitti dataset path");
+void LoadKittiImagesTimestamps(const std::string &str_path_to_sequence,
+                               std::vector<std::string> &str_image_left_vec_path,
+                               std::vector<std::string> &str_image_right_vec_path,
+                               std::vector<double> &timestamps_vec);
+class SensorManager
+{
+ public:
+  explicit SensorManager(viohw::WorldManager *slam_manager) : slam_world_(slam_manager) {
+    LOG(INFO) << "Sensor Manager is create.";
+    LoadKittiImagesTimestamps(fLS::FLAGS_kitti_dataset_path, image_left_vec_path,
+                              image_right_vec_path, vec_timestamp);
+  };
+
+  ~SensorManager() = default;
+
+  void syncProcess() {
+    LOG(INFO) << "Start the measurement reader thread";
+    const size_t num_images = image_left_vec_path.size();
+    for (int ni = 0; ni < num_images; ni++) {
+      if (slam_world_->getParams()->slam_setting_.stereo_mode_) {
+        std::lock_guard<std::mutex> lock(img_mutex_);
+        double timestamp = vec_timestamp[ni];
+        cv::Mat image0 = cv::imread(image_left_vec_path[ni], cv::IMREAD_GRAYSCALE);
+        cv::Mat image1 = cv::imread(image_right_vec_path[ni], cv::IMREAD_GRAYSCALE);
+        assert(!image0.empty() && !image1.empty());
+        slam_world_->addNewStereoImages(timestamp, image0, image1);
+      }
+      std::chrono::milliseconds dura(40);
+      std::this_thread::sleep_for(dura);
+    }
+  }
+
+ private:
+  std::queue<cv::Mat> img0_buf_;
+  std::queue<cv::Mat> img1_buf_;
+  std::mutex img_mutex_;
+  std::vector<std::string> image_left_vec_path, image_right_vec_path;
+  std::vector<double> vec_timestamp;
+  viohw::WorldManager *slam_world_;
+};
+
+int main(int argc, char **argv) {
+  google::InitGoogleLogging("hello_world_vio");
+  FLAGS_stderrthreshold = google::INFO;
+  FLAGS_colorlogtostderr = true;
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  ros::init(argc, argv, "hello_world_vio");
+
+  auto params = std::make_shared<viohw::Setting>(FLAGS_config_file_path);
+  std::cout << *params << std::endl;
+  viohw::WorldManager world_manager(params);
+  // Start the SLAM thread
+  std::thread estimate_thread(&viohw::WorldManager::run, &world_manager);
+
+  SensorManager sensor_manager(&world_manager);
+  // Start the sensor measurement grab thread
+  std::thread sensor_grab_thread(&SensorManager::syncProcess, &sensor_manager);
+
+  ros::spin();
+  return 0;
+}
+
+inline void LoadKittiImagesTimestamps(const std::string &str_path_to_sequence,
+                                      std::vector<std::string> &str_image_left_vec_path,
+                                      std::vector<std::string> &str_image_right_vec_path,
+                                      std::vector<double> &timestamps_vec) {
+  using namespace std;
+  string strPathTimeFile = str_path_to_sequence + "/times.txt";
+
+  std::ifstream fTimes(strPathTimeFile, ios::in | ios::app);
+
+  if (!fTimes.is_open()) LOG(FATAL) << "Open Failed";
+  while (!fTimes.eof()) {
+    string s;
+    getline(fTimes, s);
+    if (!s.empty()) {
+      stringstream ss;
+      ss << s;
+      double t;
+      ss >> t;
+      timestamps_vec.push_back(t);
+    }
+  }
+
+  string strPrefixLeft = str_path_to_sequence + "/image_0/";
+  string strPrefixRight = str_path_to_sequence + "/image_1/";
+
+  const size_t nTimes = timestamps_vec.size();
+  str_image_left_vec_path.resize(nTimes);
+  str_image_right_vec_path.resize(nTimes);
+
+  for (int i = 0; i < nTimes; i++) {
+    stringstream ss;
+    ss << setfill('0') << setw(6) << i;
+    str_image_left_vec_path[i] = strPrefixLeft + ss.str() + ".png";
+    str_image_right_vec_path[i] = strPrefixRight + ss.str() + ".png";
+  }
+  fTimes.close();
+}

vio_hw/params/euroc_stereo_imu.yaml → vio_hw/params/euroc/euroc_stereo_imu.yaml

@@ -19,8 +19,8 @@ FeatureAndTracker:
   feature.Type: 0
   tracker.Type: 0
   # common params
-  max.kps.num: 200
-  max.kps.distance: 30
+  max.kps.num: 300
+  max.kps.distance: 20
   use.clahe: 1
   clahe.val: 3
   track.keyframetoframe: 0

vio_hw/params/kitti/kitti_00-02.yaml

@@ -0,0 +1,75 @@
+%YAML 1.0
+---
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters.
+#--------------------------------------------------------------------------------------------
+
+Camera:
+  topic.left.right: [ "/cam0/image_raw", "/cam1/image_raw" ]
+  model.left.right: [ "pinhole","pinhole" ]
+  left.resolution: [ 1241, 376 ]
+  right.resolution: [ 1241, 376 ]
+  left.K: [718.856, 718.856, 607.1928, 185.2157]
+  right.K: [718.856, 718.856, 607.1928, 185.2157]
+  left.distortion.coefficient: [0.0, 0.0, 0.0, 0.0]
+  right.distortion.coefficient: [0.0, 0.0, 0.0, 0.0]
+
+FeatureAndTracker:
+  feature.Type: 0
+  tracker.Type: 0
+  # common params
+  max.kps.num: 500
+  max.kps.distance: 20
+  use.clahe: 1
+  clahe.val: 3
+  track.keyframetoframe: 0
+  use.brief: 1
+  # param for gftt
+  kp.quality.level: 0.001
+  # param for KLT
+  klt.use.prior: 1
+  klt.win.size: 9
+  klt.pyr.level: 3
+
+  klt.max.iter: 30
+  klt.max.px.precision: 0.01
+  klt.max.fb.dist: 0.5
+  klt.err: 30.
+
+IMU:
+  topic: "/imu0"
+  acc_n: 0.1
+  acc_w: 0.2
+  gyr_n: 0.3
+  gyr_w: 0.4
+
+SLAM:
+  stereo.mode: 1
+  use.imu: 1
+  force.realtime: 1
+  use.Rviz: 1
+  use.Pangolin: 0
+
+# Camera Extrinsic parameters T_b_ci ( v_b = T_b_ci * v_ci )
+Extrinsic:
+  camera.num: 2
+  body_T_cam0: !!opencv-matrix
+    rows: 4
+    cols: 4
+    dt: d
+    data: [1., 0., 0., 0.,
+           0., 1., 0., 0.,
+           0., 0., 1., 0.,
+           0., 0., 0., 1.]
+
+  body_T_cam1: !!opencv-matrix
+    rows: 4
+    cols: 4
+    dt: d
+    data: [1., 0., 0., 0.537165718864418,
+           0., 1., 0., 0.,
+           0., 0., 1., 0.,
+           0., 0., 0., 1.]
+
+