Merge branch 'master' into develop_v2.7

.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
 

.gitignore

@@ -7,3 +7,4 @@ doxgen_generated
 *.swp
 *.swo
 doc
+.vscode

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/

Doxyfile

@@ -843,7 +843,7 @@ EXCLUDE_SYMLINKS       = NO
 # Note that the wildcards are matched against the file with absolute path, so to
 # exclude all test directories for example use the pattern */test/*
 
-EXCLUDE_PATTERNS       = */doxygen_generated/* */catkin_generated/* */cmake-build-debug/* */plot/* */ov_data/* .github  .idea
+EXCLUDE_PATTERNS       = */ov_data/* */ov_eval/data/sim/*  */doxygen_generated/* */catkin_generated/* */cmake-build-debug/* */plot/* .github  .idea
 
 # The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
 # (namespaces, classes, functions, etc.) that should be excluded from the
@@ -880,10 +880,7 @@ EXAMPLE_RECURSIVE      = NO
 # that contain images that are to be included in the documentation (see the
 # \image command).
 
-IMAGE_PATH             = ./ \
-                         docs/ \
-                         docs/img/ \
-                         docs/youtube/
+IMAGE_PATH             = docs/img/ docs/youtube/
 
 # The INPUT_FILTER tag can be used to specify a program that doxygen should
 # invoke to filter for each input file. Doxygen will invoke the filter program

ReadMe.md

@@ -17,10 +17,12 @@ details on what the system supports.
 * Github project page - https://github.com/rpng/open_vins
 * Documentation - https://docs.openvins.com/
 * Getting started guide - https://docs.openvins.com/getting-started.html
-* Publication reference - http://udel.edu/~pgeneva/downloads/papers/c10.pdf
+* Publication reference - https://pgeneva.com/downloads/papers/Geneva2020ICRA.pdf
 
 ## News / Events
 
+* **July 14, 2022** - Improved feature extraction logic for >100hz tracking, some bug fixes and updated scripts. See v2.6.1 [PR#259](https://github.com/rpng/open_vins/pull/259).
+* **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
@@ -79,25 +81,22 @@ details on what the system supports.
     * Sparse feature SLAM features
 * Visual tracking support
     * Monocular camera
-    * Stereo camera
-    * Binocular camera
+    * Stereo camera (synchronized)
+    * Binocular cameras (synchronized)
     * 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 +106,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 +130,9 @@ details on what the system supports.
 <a href="http://www.youtube.com/watch?v=MCzTF9ye2zw">
    <img src="https://raw.githubusercontent.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://raw.githubusercontent.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 +144,10 @@ details on what the system supports.
 <a href="http://www.youtube.com/watch?v=ExPIGwORm4E">
    <img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/ExPIGwORm4E.jpg" width="120" height="90" />
 </a>
+<a href="http://www.youtube.com/watch?v=lXHl-qgLGl8">
+   <img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/lXHl-qgLGl8.jpg" width="120" height="90" />
+</a>
+
 
 
 ## Credit / Licensing
@@ -158,6 +168,7 @@ following:
 }
 ```
 
-The codebase is licensed under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt).
+The codebase and documentation is licensed under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt).
+You must preserve the copyright and license notices in your derivative work and make available the complete source code with modifications under the same license ([see this](https://choosealicense.com/licenses/gpl-3.0/); this is not legal advice).
 
 

config/euroc_mav/estimator_config.yaml

@@ -10,14 +10,14 @@ max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (
 calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
 calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
 calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized
-calib_imu_intrinsics: false # if imu intrinsics should be calibrated
-calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity should be calibrated
+calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix)
+calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated
 
 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
 
@@ -27,7 +27,7 @@ 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
+try_zupt: false
 zupt_chi2_multipler: 0 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
 zupt_noise_multiplier: 50
@@ -37,10 +37,27 @@ 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_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution)
+init_max_features: 75 # how many features to track during initialization (saves on computation)
+
+init_dyn_use: false # if dynamic initialization should be used
+init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended)
+init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE)
+init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in
+init_dyn_mle_max_threads: 6 # how many threads the MLE should use
+init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced)
+init_dyn_min_deg: 10.0 # orientation change needed to try to init
+
+init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by
+init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by
+init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by
+init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by
+init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion
+
+init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess
+init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess
 
 # ==================================================================
 # ==================================================================
@@ -60,13 +77,14 @@ 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
-grid_x: 20
-grid_y: 20
-min_px_dist: 15
-knn_ratio: 0.70
+use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching
+num_pts: 200 # number of points (per camera) we will extract and try to track
+fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive)
+grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking)
+grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking)
+min_px_dist: 10 # distance between features (features near each other provide less information)
+knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches
+track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz)
 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/euroc_mav/kalibr_imucam_chain.yaml

@@ -1,7 +1,7 @@
 %YAML:1.0
 
 cam0:
-  T_imu_cam: #rotation from camera to IMU, position of IMU in camera
+  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
     - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975]
     - [0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768]
     - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949]
@@ -14,7 +14,7 @@ cam0:
   resolution: [752, 480]
   rostopic: /cam0/image_raw
 cam1:
-  T_imu_cam: #rotation from camera to IMU, position of IMU in camera
+  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
     - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556]
     - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024]
     - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038]