Merge pull request #1 from ori-drs/dev/upstream-sync

Dev/upstream sync

.github/workflows/build.yml

@@ -14,7 +14,8 @@ jobs:
         uses: actions/checkout@v2
       - name: Installing Ceres Solver
         run: |
-          sudo apt-get update
+          sudo apt-get update -y
+          sudo apt install -y libunwind-dev
           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
@@ -27,7 +28,7 @@ jobs:
 #          cd ../../
       - name: Configure and Build
         run: |
-          sudo apt-get update
+          sudo apt-get update -y
           sudo apt-get install -y libeigen3-dev libopencv-dev libboost-all-dev
           mkdir build
           cd build

.github/workflows/build_ros2.yml

@@ -49,3 +49,24 @@ jobs:
       - name: Run OpenVINS Simulation!
         run: |
           docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml"
+  build_2204:
+    name: "ROS2 Ubuntu 22.04"
+    runs-on: ubuntu-latest
+    steps:
+      - name: Code Checkout
+        uses: actions/checkout@v2
+      - name: Create Workspace and Docker Image
+        run: |
+          export REPO=$(basename $GITHUB_REPOSITORY) &&
+          cd $GITHUB_WORKSPACE/.. && mkdir src/ &&
+          mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ &&
+          docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros2_22_04 .
+      - name: Echo Enviroment
+        run: |
+          docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION"
+      - name: Run Build in Docker
+        run: |
+          docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && colcon build"
+#      - name: Run OpenVINS Simulation!
+#        run: |
+#          docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml"

Dockerfile_ros2_22_04

@@ -0,0 +1,47 @@
+FROM osrf/ros:humble-desktop
+
+# =========================================================
+# =========================================================
+
+# Are you are looking for how to use this docker file?
+#   - https://docs.openvins.com/dev-docker.html
+#   - https://docs.docker.com/get-started/
+#   - http://wiki.ros.org/docker/Tutorials/Docker
+
+# =========================================================
+# =========================================================
+
+# Dependencies we use, catkin tools is very good build system
+# 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.10 installed on it...
+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/
+# RUN sed -i '6i\source "/catkin_ws/install/setup.bash"\' /ros_entrypoint.sh
+RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \
+    gdb clang cmake rsync tar python3 && apt-get clean
+RUN ( \
+    echo 'LogLevel DEBUG2'; \
+    echo 'PermitRootLogin yes'; \
+    echo 'PasswordAuthentication yes'; \
+    echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \
+  ) > /etc/ssh/sshd_config_test_clion \
+  && mkdir /run/sshd
+RUN useradd -m user && yes password | passwd user
+RUN usermod -s /bin/bash user
+CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"]
+

Doxyfile

@@ -1761,7 +1761,7 @@ LATEX_BIB_STYLE        = plainnat
 # The default value is: NO.
 # This tag requires that the tag GENERATE_LATEX is set to YES.
 
-LATEX_TIMESTAMP        = NO
+LATEX_TIMESTAMP        = YES
 
 #---------------------------------------------------------------------------
 # Configuration options related to the RTF output

ReadMe.md

@@ -44,6 +44,9 @@ details on what the system supports.
 
 ## News / Events
 
+* **May 11, 2023** - Inertial intrinsic support released as part of v2.7 along with a few bug fixes and improvements to stereo KLT tracking. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.7) for details.
+* **April 15, 2023** - Minor update to v2.6.3 to support incremental feature triangulation of active features for downstream applications, faster zero-velocity update, small bug fixes, some example realsense configurations, and cached fast state prediction. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.6.3) for details.
+* **April 3, 2023** - We have released a monocular plane-aided VINS, termed [ov_plane](https://github.com/rpng/ov_plane), which leverages the OpenVINS project. Both now support the released [Indoor AR Table](https://github.com/rpng/ar_table_dataset) dataset.
 * **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) and v2.6.2 [PR#264](https://github.com/rpng/open_vins/pull/264).
 * **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
@@ -99,6 +102,7 @@ details on what the system supports.
     * Camera to IMU transform
     * Camera to IMU time offset
     * Camera intrinsics
+    * Inertial intrinsics (including g-sensitivity)
 * Environmental SLAM feature
     * OpenCV ARUCO tag SLAM features
     * Sparse feature SLAM features
@@ -110,11 +114,18 @@ details on what the system supports.
     * Masked tracking
 * 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
+* Out of the box evaluation on EuRocMav, TUM-VI, UZH-FPV, KAIST Urban and other VIO datasets
 * Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..)
 
 ## Codebase Extensions
 
+* **[ov_plane](https://github.com/rpng/ov_plane)** - A real-time monocular visual-inertial odometry (VIO) system which leverages
+  environmental planes. At the core it presents an efficient robust monocular-based plane detection algorithm which does
+  not require additional sensing modalities such as a stereo, depth camera or neural network. The plane detection and tracking
+  algorithm enables real-time regularization of point features to environmental planes which are either maintained in the state
+  vector as long-lived planes, or marginalized for efficiency. Planar regularities are applied to both in-state SLAM and
+  out-of-state MSCKF point features, enabling long-term point-to-plane loop-closures due to the large spacial volume of planes.
+
 * **[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

config/euroc_mav/estimator_config.yaml

@@ -3,15 +3,15 @@
 verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
 
 use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
-use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
-
+integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
 use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs
 max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
 
 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 (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
@@ -30,17 +30,17 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"
 try_zupt: false
 zupt_chi2_multipler: 0 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
-zupt_noise_multiplier: 50
-zupt_max_disparity: 1.5 # set to 0 for only imu-based
-zupt_only_at_beginning: true
+zupt_noise_multiplier: 10
+zupt_max_disparity: 0.5 # set to 0 for only imu-based
+zupt_only_at_beginning: false
 
 # ==================================================================
 # ==================================================================
 
 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: 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_max_features: 50 # 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)

config/euroc_mav/kalibr_imu_chain.yaml

@@ -10,7 +10,35 @@ imu0:
   accelerometer_random_walk: 3.0000e-3    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
   gyroscope_noise_density: 1.6968e-04     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
   gyroscope_random_walk: 1.9393e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
-  model: calibrated
   rostopic: /imu0
   time_offset: 0.0
-  update_rate: 200.0
+  update_rate: 200.0
+  # three different modes supported:
+  # "calibrated" (same as "kalibr"), "kalibr", "rpng"
+  model: "kalibr"
+  # how to get from Kalibr imu.yaml result file:
+  #   - Tw is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
+  #   - Ta is imu0:accelerometers:M:
+  #   - R_IMUtoACC not used by Kalibr
+  #   - Tg is imu0:gyroscopes:A:
+  Tw:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoGYRO:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Ta:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoACC:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Tg:
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]

config/kaist/estimator_config.yaml

@@ -1,19 +1,19 @@
 %YAML:1.0 # need to specify the file type at the top!
 
-verbosity: "DEBUG" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
-
-use_fej: true # if we should use first-estimate Jacobians (enable for good consistency)
-use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
+verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
 
+use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
+integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
 use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints
-max_cameras: 2
+max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
 
 calib_cam_extrinsics: false # degenerate motion
 calib_cam_intrinsics: true
 calib_cam_timeoffset: true # degenerate motion
+calib_imu_intrinsics: false
+calib_imu_g_sensitivity: false
 
-max_clones: 8
+max_clones: 11
 max_slam: 50
 max_slam_in_update: 25
 max_msckf_in_update: 50
@@ -28,9 +28,9 @@ 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.25 # set to 0 for only disp-based
+zupt_chi2_multipler: 0.5 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
-zupt_noise_multiplier: 5
+zupt_noise_multiplier: 1
 zupt_max_disparity: 0.4 # set to 0 for only imu-based
 zupt_only_at_beginning: false
 
@@ -40,7 +40,7 @@ zupt_only_at_beginning: false
 init_window_time: 2.0
 init_imu_thresh: 0.5
 init_max_disparity: 1.5
-init_max_features: 75
+init_max_features: 50
 
 init_dyn_use: true
 init_dyn_mle_opt_calib: false

config/kaist/kalibr_imu_chain.yaml

@@ -18,7 +18,35 @@ imu0:
   accelerometer_random_walk: 1.0000e-04    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
   gyroscope_noise_density: 1.7453e-04      # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
   gyroscope_random_walk:  1.0000e-05      # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
-  model: calibrated
   rostopic: /imu/data_raw
   time_offset: 0.0
-  update_rate: 500.0
+  update_rate: 500.0
+  # three different modes supported:
+  # "calibrated" (same as "kalibr"), "kalibr", "rpng"
+  model: "kalibr"
+  # how to get from Kalibr imu.yaml result file:
+  #   - Tw is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
+  #   - Ta is imu0:accelerometers:M:
+  #   - R_IMUtoACC not used by Kalibr
+  #   - Tg is imu0:gyroscopes:A:
+  Tw:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoGYRO:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Ta:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoACC:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Tg:
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]

config/kaist/kalibr_imucam_chain.yaml

@@ -2,9 +2,9 @@
 
 cam0:
   T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
-    - [-0.00413,-0.01966,0.99980,1.73944]
-    - [-0.99993,-0.01095,-0.00435,0.27803]
-    - [0.01103,-0.99975,-0.01962,-0.08785]
+    - [-0.00681,-0.01532,0.99987,1.71239]
+    - [-0.99998,0.00033,-0.00680,0.24740]
+    - [-0.00023,-0.99988,-0.01532,-0.11589]
     - [0.0, 0.0, 0.0, 1.0]
   cam_overlaps: [1]
   camera_model: pinhole
@@ -16,9 +16,9 @@ cam0:
 
 cam1:
   T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
-    - [-0.00768,-0.01509,0.99986,1.73376]
-    - [-0.99988,-0.01305,-0.00788,-0.19706]
-    - [0.01317,-0.99980,-0.01499,-0.08271]
+    - [-0.01036,-0.01075,0.99990,1.70544]
+    - [-0.99994,-0.00178,-0.01038,-0.22770]
+    - [0.00189,-0.99994,-0.01073,-0.11611]
     - [0.0, 0.0, 0.0, 1.0]
   cam_overlaps: [0]
   camera_model: pinhole
@@ -27,3 +27,4 @@ cam1:
   intrinsics: [8.1378205539589999e+02,8.0852165574269998e+02,6.1386419539320002e+02,2.4941049348650000e+02]
   resolution: [1280, 560]
   rostopic: /stereo/right/image_raw
+

config/kaist_vio/estimator_config.yaml

@@ -3,15 +3,15 @@
 verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
 
 use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
-use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
-
+integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
 use_stereo: true # NEED TO USE STEREO! OTHERWISE CAN'T RECOVER SCALE!!!!!! DEGENERATE MOTION!!!
 max_cameras: 2 # NEED TO USE STEREO! OTHERWISE CAN'T RECOVER SCALE!!!!!! DEGENERATE MOTION!!!
 
 calib_cam_extrinsics: false # disable since this is a degenerate motion
 calib_cam_intrinsics: true
 calib_cam_timeoffset: false # disable since this is a degenerate motion
+calib_imu_intrinsics: false
+calib_imu_g_sensitivity: false
 
 max_clones: 11 # how many clones in the sliding window
 max_slam: 50 # number of features in our state vector
@@ -40,7 +40,7 @@ zupt_only_at_beginning: false
 init_window_time: 2.0 # how many seconds to collect initialization information
 init_imu_thresh: 0.60 # threshold for variance of the accelerometer to detect a "jerk" in motion
 init_max_disparity: 5.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_max_features: 50 # how many features to track during initialization (saves on computation)
 
 init_dyn_use: false
 init_dyn_mle_opt_calib: false