Skip to content

Commit

Permalink
Merge pull request #232 from rpng/develop_v2.6
Browse files Browse the repository at this point in the history
Develop v2.6 - Dynamic Initialization and Async Publishing
  • Loading branch information
goldbattle authored Mar 21, 2022
2 parents ed7fe55 + 4fa753c commit 06cc895
Show file tree
Hide file tree
Showing 201 changed files with 700,445 additions and 303,436 deletions.
24 changes: 20 additions & 4 deletions .github/workflows/build.yml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@ name: ROS Free Workflow

on:
push:
branches: [ master]
branches: [ master ]
pull_request:

jobs:
Expand All @@ -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
16 changes: 15 additions & 1 deletion Dockerfile_ros1_16_04
Original file line number Diff line number Diff line change
Expand Up @@ -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/
Expand Down
13 changes: 12 additions & 1 deletion Dockerfile_ros1_18_04
Original file line number Diff line number Diff line change
Expand Up @@ -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/
Expand Down
13 changes: 12 additions & 1 deletion Dockerfile_ros1_20_04
Original file line number Diff line number Diff line change
Expand Up @@ -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/
Expand Down
13 changes: 12 additions & 1 deletion Dockerfile_ros2_18_04
Original file line number Diff line number Diff line change
Expand Up @@ -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/
Expand Down
13 changes: 12 additions & 1 deletion Dockerfile_ros2_20_04
Original file line number Diff line number Diff line change
Expand Up @@ -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/
Expand Down
39 changes: 24 additions & 15 deletions ReadMe.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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

Expand All @@ -127,6 +129,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">
Expand All @@ -138,6 +143,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-qgLGl">
<img src="https://raw.githubusercontent.com/rpng/open_vins/master/docs/youtube/lXHl-qgLGl.jpg" width="120" height="90" />
</a>



## Credit / Licensing
Expand Down
27 changes: 22 additions & 5 deletions config/euroc_mav/estimator_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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]

# ==================================================================
# ==================================================================
Expand All @@ -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
Expand Down
53 changes: 36 additions & 17 deletions config/kaist/estimator_config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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]


# ==================================================================
# ==================================================================
Expand All @@ -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

Expand Down
4 changes: 2 additions & 2 deletions config/kaist/kalibr_imucam_chain.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Loading

0 comments on commit 06cc895

Please sign in to comment.