Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Develop v2.6.1 #259

Merged
merged 27 commits into from
Jul 18, 2022
Merged

Develop v2.6.1 #259

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
27 commits
Select commit Hold shift + click to select a range
45d1a5e
fix first imu reading not getting passed in simulation
goldbattle Jun 20, 2022
1d4cf85
fix getting sim save param in ros2 (closes #249)
goldbattle Jun 20, 2022
316acbc
build with release by default
goldbattle Jun 20, 2022
27648a5
ov_core predeclare
goldbattle Jun 20, 2022
e6bc0f2
ov_init predeclares
goldbattle Jun 20, 2022
34919ff
switch to sharedptr, explict jacobSVD sizes
goldbattle Jun 20, 2022
4129770
ov_msckf forward declare
goldbattle Jun 20, 2022
eab1771
test performing extraction based on a gridblock basis instead of whol…
goldbattle Jun 21, 2022
840d3b5
fix dynamic init coeff size, correctly marg SLAM feature on chi2 fail…
goldbattle Jun 21, 2022
08ce287
fix tracking param name (should be if stereo is desired)
goldbattle Jun 22, 2022
b2a5f8d
update stereo klt to use new grider
goldbattle Jun 22, 2022
ef3d15e
new serial reader code
goldbattle Jul 5, 2022
70e6a0c
fix imu deleting bug (don't use featdb), and rename ov_init simulator
goldbattle Jul 11, 2022
c00a7ee
multi-thread image publishing, small fix for omega cov in odom fastpr…
goldbattle Jul 12, 2022
658544d
check disparity for movement and still for static init now
goldbattle Jul 14, 2022
cd64295
update configs to handle new init (static by default)
goldbattle Jul 15, 2022
65d98a0
update launch
goldbattle Jul 15, 2022
d1459da
update scripts
goldbattle Jul 15, 2022
f3c3259
update link to kaistvio
goldbattle Jul 15, 2022
749d10e
update readme [skip ci]
goldbattle Jul 15, 2022
53b29dc
add missing headers
goldbattle Jul 15, 2022
8e0fb58
more missing std headers
goldbattle Jul 15, 2022
0275f2a
default setting to save sim to file
goldbattle Jul 15, 2022
16b4790
fix eval bug for NEES if using alignment
goldbattle Jul 15, 2022
1c60257
set bigger prior for better realworld nees, also extract feats more f…
goldbattle Jul 18, 2022
29d7f71
fix unused warning
goldbattle Jul 18, 2022
d6f5ba1
more config comments [skip ci]
goldbattle Jul 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,3 +7,4 @@ doxgen_generated
*.swp
*.swo
doc
.vscode
3 changes: 2 additions & 1 deletion ReadMe.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,11 @@ 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.
Expand Down
55 changes: 28 additions & 27 deletions config/euroc_mav/estimator_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -39,24 +39,25 @@ zupt_only_at_beginning: true

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: 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]
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

# ==================================================================
# ==================================================================
Expand All @@ -76,14 +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: 200
fast_threshold: 20
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 21.0
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
Expand Down
35 changes: 18 additions & 17 deletions config/kaist/estimator_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,17 +9,16 @@ use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints
max_cameras: 2

calib_cam_extrinsics: false
calib_cam_extrinsics: false # degenerate motion
calib_cam_intrinsics: true
calib_cam_timeoffset: true
calib_cam_timeoffset: true # degenerate motion

max_clones: 12
max_clones: 8
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 50
dt_slam_delay: 1

#gravity_mag: 9.79858
gravity_mag: 9.81

feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH"
Expand All @@ -29,7 +28,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
zupt_chi2_multipler: 1 # set to 0 for only disp-based
zupt_chi2_multipler: 0.25 # set to 0 for only disp-based
zupt_max_velocity: 0.1
zupt_noise_multiplier: 5
zupt_max_disparity: 0.4 # set to 0 for only imu-based
Expand All @@ -39,16 +38,17 @@ zupt_only_at_beginning: false
# ==================================================================

init_window_time: 2.0
init_imu_thresh: 0.5 #0.5
init_imu_thresh: 0.5
init_max_disparity: 1.5
init_max_features: 25
init_max_features: 75

init_dyn_use: true
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_min_deg: 5.0 # traj is mostly straight line motion

init_dyn_inflation_ori: 10
init_dyn_inflation_vel: 100
Expand Down Expand Up @@ -78,18 +78,19 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
# we have a KLT and descriptor based (KLT is better implemented...)
use_klt: true
num_pts: 200
fast_threshold: 50
grid_x: 20
grid_y: 15
min_px_dist: 30
fast_threshold: 30
grid_x: 5
grid_y: 5
min_px_dist: 20
knn_ratio: 0.65
track_frequency: 21.0
track_frequency: 31.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE

fi_max_dist: 500
fi_max_baseline: 800
fi_min_dist: 0.25
fi_max_dist: 150.0
fi_max_baseline: 200
fi_max_cond_number: 20000
fi_triangulate_1d: false

Expand All @@ -103,9 +104,9 @@ downsize_aruco: true
# ==================================================================

# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_sigma_px: 1.5
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_sigma_px: 1.5
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
Expand Down
19 changes: 10 additions & 9 deletions config/kaist/kalibr_imu_chain.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,22 +1,23 @@
%YAML:1.0 # need to specify the file type at the top!


# MTI-100 series converted from data sheet, guess on bias random walk
# MTI-100 series converted from datasheet, guess on bias random walk
# https://www.xsens.com/hubfs/Downloads/usermanual/MTi_usermanual.pdf

imu0:
T_i_b:
- [1.0, 0.0, 0.0, 0.0]
- [0.0, 1.0, 0.0, 0.0]
- [0.0, 0.0, 1.0, 0.0]
- [0.0, 0.0, 0.0, 1.0]
accelerometer_noise_density: 2.0000e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 3.0000e-03 # [ 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 )
# <param name="gyroscope_noise_density" type="double" value="1.7453e-04" /> <!-- 1.7453e-04 -->
# <param name="gyroscope_random_walk" type="double" value="1.0000e-05" />
# <param name="accelerometer_noise_density" type="double" value="5.8860e-03" /> <!-- 5.8860e-04 -->
# <param name="accelerometer_random_walk" type="double" value="1.0000e-04" />
# accelerometer_noise_density: 2.0000e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
# accelerometer_random_walk: 3.0000e-03 # [ 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 )
accelerometer_noise_density: 5.8860e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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
Expand Down
3 changes: 2 additions & 1 deletion config/kaist/kalibr_imucam_chain.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ cam0:
intrinsics: [8.1690378992770002e+02,8.1156803828490001e+02,6.0850726281690004e+02,2.6347599764440002e+02]
resolution: [1280, 560]
rostopic: /stereo/left/image_raw

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]
Expand All @@ -25,4 +26,4 @@ cam1:
distortion_model: radtan
intrinsics: [8.1378205539589999e+02,8.0852165574269998e+02,6.1386419539320002e+02,2.4941049348650000e+02]
resolution: [1280, 560]
rostopic: /stereo/right/image_raw
rostopic: /stereo/right/image_raw
19 changes: 10 additions & 9 deletions config/kaist_vio/estimator_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,18 +16,18 @@ calib_cam_timeoffset: false # disable since this is a degenerate motion
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
max_msckf_in_update: 50 # how many MSCKF features to use in the update
dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)

gravity_mag: 9.81 # magnitude of gravity in this location

feat_rep_msckf: "GLOBAL_3D"
feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH"
feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
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.02
zupt_noise_multiplier: 10
Expand All @@ -38,10 +38,11 @@ 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: 0.20 # max disparity to consider the platform stationary (dependent on resolution)
init_max_features: 25 # how many features to track during initialization (saves on computation)
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_dyn_use: false
init_dyn_mle_opt_calib: false
init_dyn_mle_max_iter: 50
init_dyn_mle_max_time: 0.05
Expand Down Expand Up @@ -79,11 +80,11 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
use_klt: true
num_pts: 200
fast_threshold: 30
grid_x: 20
grid_y: 20
grid_x: 5
grid_y: 5
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 10.0
track_frequency: 31.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
16 changes: 8 additions & 8 deletions config/kaist_vio/kalibr_imu_chain.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,14 @@ imu0:
- [0.0, 1.0, 0.0, 0.0]
- [0.0, 0.0, 1.0, 0.0]
- [0.0, 0.0, 0.0, 1.0]
accelerometer_noise_density: 0.00333388 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 0.00047402 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
gyroscope_noise_density: 0.00005770 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
gyroscope_random_walk: 0.00001565 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
# accelerometer_noise_density: 0.07 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
# accelerometer_random_walk: 0.009 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
# gyroscope_noise_density: 0.001 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
# gyroscope_random_walk: 0.0003 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
# accelerometer_noise_density: 0.00333388 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
# accelerometer_random_walk: 0.00047402 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
# gyroscope_noise_density: 0.00005770 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
# gyroscope_random_walk: 0.00001565 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
accelerometer_noise_density: 0.07 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 0.009 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
gyroscope_noise_density: 0.001 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
gyroscope_random_walk: 0.0003 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
model: calibrated
rostopic: /mavros/imu/data
time_offset: 0.0
Expand Down
21 changes: 11 additions & 10 deletions config/rpng_aruco/estimator_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -39,9 +39,10 @@ zupt_only_at_beginning: true

init_window_time: 2.0
init_imu_thresh: 1.2
init_max_disparity: 1.5
init_max_features: 25
init_max_disparity: 2.0
init_max_features: 75

init_dyn_use: false
init_dyn_mle_opt_calib: false
init_dyn_mle_max_iter: 50
init_dyn_mle_max_time: 0.05
Expand Down Expand Up @@ -77,9 +78,9 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
use_klt: true
num_pts: 150
fast_threshold: 30
grid_x: 20
grid_y: 20
min_px_dist: 30
grid_x: 5
grid_y: 5
min_px_dist: 20
knn_ratio: 0.85
track_frequency: 21.0
downsample_cameras: false
Expand All @@ -96,12 +97,12 @@ downsize_aruco: true
# ==================================================================

# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_sigma_px: 1.5
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_sigma_px: 1.5
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 8
up_aruco_sigma_px: 2.0
up_aruco_chi2_multipler: 10

# masks for our images
use_mask: false
Expand Down
Loading