Develop v2.6 - Dynamic Initialization and Async Publishing

.github/workflows/build.yml

@@ -2,7 +2,7 @@ name: ROS Free Workflow
 
 on:
   push:
-    branches: [ master]
+    branches: [ master ]
   pull_request:
 
 jobs:
@@ -12,9 +12,25 @@ jobs:
     steps:
       - name: Code Checkout
         uses: actions/checkout@v2
+      - name: Installing Ceres Solver
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
+#          cd ..
+#          git clone https://ceres-solver.googlesource.com/ceres-solver
+#          cd ceres-solver
+#          git checkout tags/2.0.0
+#          mkdir build
+#          cd build
+#          cmake ..
+#          sudo make install
+#          cd ../../
       - name: Configure and Build
         run: |
-          sudo apt-get update && sudo apt-get install -y libeigen3-dev libopencv-dev libboost-all-dev &&
-          mkdir build && cd build &&
-          cmake ../ov_msckf/ && make
+          sudo apt-get update
+          sudo apt-get install -y libeigen3-dev libopencv-dev libboost-all-dev
+          mkdir build
+          cd build
+          cmake ../ov_msckf/
+          make
 

Dockerfile_ros1_16_04

@@ -16,8 +16,22 @@ FROM osrf/ros:kinetic-desktop-full
 RUN apt-get update && apt-get install -y libeigen3-dev nano git
 RUN sudo apt-get install -y python-catkin-tools
 
+# Ceres solver install and setup
+# NOTE: need to use an older version as eigen is very outdated here!!!
+# NOTE: https://github.com/ceres-solver/ceres-solver/issues/541
+RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
+# ENV CERES_VERSION="2.0.0"
+# RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \
+#     cd ceres-solver && \
+#     git checkout tags/${CERES_VERSION} && \
+#     git reset --hard e51e9b46f6 && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j$(nproc) install && \
+#     rm -rf ../../ceres-solver
+
 # Seems this has Python 2.7 installed on it...
-RUN sudo apt-get install -y python2.7-dev python-matplotlib python-numpy python-psutil python-tk
+RUN apt-get update && apt-get install -y python2.7-dev python-matplotlib python-numpy python-psutil python-tk
 
 # Install deps needed for clion remote debugging
 # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/

Dockerfile_ros1_18_04

@@ -16,8 +16,19 @@ FROM osrf/ros:melodic-desktop-full
 RUN apt-get update && apt-get install -y libeigen3-dev nano git
 RUN sudo apt-get install -y python-catkin-tools
 
+# Ceres solver install and setup
+RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
+# ENV CERES_VERSION="2.0.0"
+# RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \
+#     cd ceres-solver && \
+#     git checkout tags/${CERES_VERSION} && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j$(nproc) install && \
+#     rm -rf ../../ceres-solver
+
 # Seems this has Python 3.6 installed on it...
-RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
+RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
 
 # Install deps needed for clion remote debugging
 # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/

Dockerfile_ros1_20_04

@@ -16,8 +16,19 @@ FROM osrf/ros:noetic-desktop-full
 RUN apt-get update && apt-get install -y libeigen3-dev nano git
 RUN sudo apt-get install -y python3-catkin-tools python3-osrf-pycommon
 
+# Ceres solver install and setup
+RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
+# ENV CERES_VERSION="2.0.0"
+# RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \
+#     cd ceres-solver && \
+#     git checkout tags/${CERES_VERSION} && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j$(nproc) install && \
+#     rm -rf ../../ceres-solver
+
 # Seems this has Python 3.8 installed on it...
-RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
+RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
 
 # Install deps needed for clion remote debugging
 # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/

Dockerfile_ros2_18_04

@@ -15,8 +15,19 @@ FROM osrf/ros:dashing-desktop
 # Also some helper utilities for fast in terminal edits (nano etc)
 RUN apt-get update && apt-get install -y libeigen3-dev nano git
 
+# Ceres solver install and setup
+RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
+# ENV CERES_VERSION="2.0.0"
+# RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \
+#     cd ceres-solver && \
+#     git checkout tags/${CERES_VERSION} && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j$(nproc) install && \
+#     rm -rf ../../ceres-solver
+
 # Seems this has Python 3.6 installed on it...
-RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
+RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
 
 # Install deps needed for clion remote debugging
 # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/

Dockerfile_ros2_20_04

@@ -15,8 +15,19 @@ FROM osrf/ros:galactic-desktop
 # Also some helper utilities for fast in terminal edits (nano etc)
 RUN apt-get update && apt-get install -y libeigen3-dev nano git
 
+# Ceres solver install and setup
+RUN sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
+# ENV CERES_VERSION="2.0.0"
+# RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \
+#     cd ceres-solver && \
+#     git checkout tags/${CERES_VERSION} && \
+#     mkdir build && cd build && \
+#     cmake .. && \
+#     make -j$(nproc) install && \
+#     rm -rf ../../ceres-solver
+
 # Seems this has Python 3.8 installed on it...
-RUN sudo apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
+RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk
 
 # Install deps needed for clion remote debugging
 # https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/

ReadMe.md

@@ -21,6 +21,7 @@ details on what the system supports.
 
 ## News / Events
 
+* **March 14, 2022** - Initial dynamic initialization open sourcing, asynchronous subscription to inertial readings and publishing of odometry, support for lower frequency feature tracking. See v2.6 [PR#232](https://github.com/rpng/open_vins/pull/232) for details.
 * **December 13, 2021** - New YAML configuration system, ROS2 support, Docker images, robust static initialization based on disparity, internal logging system to reduce verbosity, image transport publishers, dynamic number of features support, and other small fixes. See
   v2.5 [PR#209](https://github.com/rpng/open_vins/pull/209) for details.
 * **July 19, 2021** - Camera classes, masking support, alignment utility, and other small fixes. See
@@ -80,24 +81,21 @@ details on what the system supports.
 * Visual tracking support
     * Monocular camera
     * Stereo camera
-    * Binocular camera
+    * Binocular (synchronized) cameras
     * KLT or descriptor based
     * Masked tracking
-* Static IMU initialization (sfm will be open sourced later)
+* Static and dynamic state initialization
 * Zero velocity detection and updates
 * Out of the box evaluation on EurocMav, TUM-VI, UZH-FPV, KAIST Urban and VIO datasets
 * Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..)
 
 ## Codebase Extensions
 
-* **[ov_secondary](https://github.com/rpng/ov_secondary)** - This is an example secondary thread which provides loop
-  closure in a loosely coupled manner for [OpenVINS](https://github.com/rpng/open_vins). This is a modification of the
-  code originally developed by the HKUST aerial robotics group and can be found in
-  their [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) repository. Here we stress that this is a
-  loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry.
-  This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to
-  camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop
-  closure detection to improve frequency.
+* **[vicon2gt](https://github.com/rpng/vicon2gt)** - This utility was created to generate groundtruth trajectories using
+  a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems.
+  Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth
+  trajectory similar to those provided by the EurocMav datasets. Performs fusion of inertial and motion capture
+  information and estimates all unknown spacial-temporal calibrations between the two sensors.
 
 * **[ov_maplab](https://github.com/rpng/ov_maplab)** - This codebase contains the interface wrapper for exporting
   visual-inertial runs from [OpenVINS](https://github.com/rpng/open_vins) into the ViMap structure taken
@@ -107,11 +105,15 @@ details on what the system supports.
   running the data through OpenVINS. Some example have been provided along with a helper script to export trajectories
   into the standard groundtruth format.
 
-* **[vicon2gt](https://github.com/rpng/vicon2gt)** - This utility was created to generate groundtruth trajectories using
-  a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems.
-  Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth
-  trajectory similar to those provided by the EurocMav datasets. Performs fusion of inertial and motion capture
-  information and estimates all unknown spacial-temporal calibrations between the two sensors.
+* **[ov_secondary](https://github.com/rpng/ov_secondary)** - This is an example secondary thread which provides loop
+  closure in a loosely coupled manner for [OpenVINS](https://github.com/rpng/open_vins). This is a modification of the
+  code originally developed by the HKUST aerial robotics group and can be found in
+  their [VINS-Fusion](https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) repository. Here we stress that this is a
+  loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry.
+  This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to
+  camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop
+  closure detection to improve frequency.
+
 
 ## Demo Videos
 
@@ -127,6 +129,9 @@ details on what the system supports.
 <a href="http://www.youtube.com/watch?v=MCzTF9ye2zw">
    <img src="https://github.com/rpng/open_vins/master/docs/youtube/MCzTF9ye2zw.jpg"  width="120" height="90"/>
 </a>
+<a href="http://www.youtube.com/watch?v=eSQLWcNrx_I">
+   <img src="https://github.com/rpng/open_vins/master/docs/youtube/eSQLWcNrx_I.jpg" width="120" height="90" />
+</a>
 <br/>
 
 <a href="http://www.youtube.com/watch?v=187AXuuGNNw">
@@ -138,6 +143,10 @@ details on what the system supports.
 <a href="http://www.youtube.com/watch?v=ExPIGwORm4E">
    <img src="https://github.com/rpng/open_vins/master/docs/youtube/ExPIGwORm4E.jpg" width="120" height="90" />
 </a>
+<a href="http://www.youtube.com/watch?v=lXHl-qgLGl">
+   <img src="https://github.com/rpng/open_vins/master/docs/youtube/lXHl-qgLGl.jpg" width="120" height="90" />
+</a>
+
 
 
 ## Credit / Licensing

config/euroc_mav/estimator_config.yaml

@@ -17,7 +17,7 @@ max_clones: 11 # how many clones in the sliding window
 max_slam: 50 # number of features in our state vector
 max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
 max_msckf_in_update: 40 # how many MSCKF features to use in the update
-dt_slam_delay: 2 # delay before initializing (helps with stability from bad initialization...)
+dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)
 
 gravity_mag: 9.81 # magnitude of gravity in this location
 
@@ -37,10 +37,26 @@ zupt_only_at_beginning: true
 # ==================================================================
 # ==================================================================
 
-init_window_time: 0.75 # how many seconds to collect initialization information
+init_window_time: 2.0 # how many seconds to collect initialization information
 init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
 init_max_disparity: 1.5 # max disparity to consider the platform stationary (dependent on resolution)
-init_max_features: 50 # how many features to track during initialization (saves on computation)
+init_max_features: 25 # how many features to track during initialization (saves on computation)
+
+init_dyn_mle_opt_calib: false
+init_dyn_mle_max_iter: 50
+init_dyn_mle_max_time: 0.05
+init_dyn_mle_max_threads: 6
+init_dyn_num_pose: 6
+init_dyn_min_deg: 10.0
+
+init_dyn_inflation_ori: 10
+init_dyn_inflation_vel: 100
+init_dyn_inflation_bg: 10
+init_dyn_inflation_ba: 100
+init_dyn_min_rec_cond: 1e-12
+
+init_dyn_bias_g: [0.0, 0.0, 0.0]
+init_dyn_bias_a: [0.0, 0.0, 0.0]
 
 # ==================================================================
 # ==================================================================
@@ -61,12 +77,13 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
 # our front-end feature tracking parameters
 # we have a KLT and descriptor based (KLT is better implemented...)
 use_klt: true
-num_pts: 250
-fast_threshold: 15
+num_pts: 200
+fast_threshold: 20
 grid_x: 20
 grid_y: 20
 min_px_dist: 15
 knn_ratio: 0.70
+track_frequency: 21.0
 downsample_cameras: false # will downsample image in half if true
 multi_threading: true # if should enable opencv multi threading
 histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE

config/kaist/estimator_config.yaml

@@ -13,13 +13,14 @@ calib_cam_extrinsics: false
 calib_cam_intrinsics: true
 calib_cam_timeoffset: true
 
-max_clones: 25
-max_slam: 10
+max_clones: 12
+max_slam: 50
 max_slam_in_update: 25
-max_msckf_in_update: 100
+max_msckf_in_update: 50
 dt_slam_delay: 1
 
-gravity_mag: 9.79858
+#gravity_mag: 9.79858
+gravity_mag: 9.81
 
 feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH"
 feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
@@ -28,19 +29,36 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"
 # zero velocity update parameters we can use
 # we support either IMU-based or disparity detection.
 try_zupt: true
-zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_chi2_multipler: 1 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
-zupt_noise_multiplier: 1
-zupt_max_disparity: 0.3 # set to 0 for only imu-based
+zupt_noise_multiplier: 5
+zupt_max_disparity: 0.4 # set to 0 for only imu-based
 zupt_only_at_beginning: false
 
 # ==================================================================
 # ==================================================================
 
-init_window_time: 1.0
-init_imu_thresh: 0.5
+init_window_time: 2.0
+init_imu_thresh: 0.5 #0.5
 init_max_disparity: 1.5
-init_max_features: 50
+init_max_features: 25
+
+init_dyn_mle_opt_calib: false
+init_dyn_mle_max_iter: 50
+init_dyn_mle_max_time: 0.05
+init_dyn_mle_max_threads: 6
+init_dyn_num_pose: 6
+init_dyn_min_deg: 0.0 # traj is mostly straight line motion
+
+init_dyn_inflation_ori: 10
+init_dyn_inflation_vel: 100
+init_dyn_inflation_bg: 10
+init_dyn_inflation_ba: 100
+init_dyn_min_rec_cond: 1e-20 # traj is mostly straight line motion
+
+init_dyn_bias_g: [0.0, 0.0, 0.0]
+init_dyn_bias_a: [0.0, 0.0, 0.0]
+
 
 # ==================================================================
 # ==================================================================
@@ -59,18 +77,19 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
 # our front-end feature tracking parameters
 # we have a KLT and descriptor based (KLT is better implemented...)
 use_klt: true
-num_pts: 300
-fast_threshold: 10
+num_pts: 200
+fast_threshold: 50
 grid_x: 20
-grid_y: 20
-min_px_dist: 20
+grid_y: 15
+min_px_dist: 30
 knn_ratio: 0.65
+track_frequency: 21.0
 downsample_cameras: false
 multi_threading: true
-histogram_method: "CLAHE" # NONE, HISTOGRAM, CLAHE
+histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
 
-fi_max_dist: 150
-fi_max_baseline: 200
+fi_max_dist: 500
+fi_max_baseline: 800
 fi_max_cond_number: 20000
 fi_triangulate_1d: false
 

config/kaist/kalibr_imucam_chain.yaml

@@ -21,8 +21,8 @@ cam1:
     - [0.0, 0.0, 0.0, 1.0]
   cam_overlaps: [0]
   camera_model: pinhole
-  distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606, 0.0003299517423931039]
+  distortion_coeffs: [-5.4921981799999998e-02,1.4243657430000001e-01,7.5412299999999996e-05,-6.7560530000000001e-04]
   distortion_model: radtan
-  intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
+  intrinsics: [8.1378205539589999e+02,8.0852165574269998e+02,6.1386419539320002e+02,2.4941049348650000e+02]
   resolution: [1280, 560]
   rostopic: /stereo/right/image_raw