pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
Иван Подмогильный
2022-08-05 08:23:25 +03:00
commit 5ecdc6abcf
387 changed files with 3010095 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
config/euroc_mav/estimator_config.yaml Normal file
View File

@@ -0,0 +1,118 @@
%YAML:1.0 # need to specify the file type at the top!
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)
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
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: 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_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
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
# ==================================================================
# ==================================================================
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]
# ==================================================================
# ==================================================================
record_timing_information: false # if we want to record timing information of the method
record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame
# if we want to save the simulation state and its diagional covariance
# use this with rosrun ov_eval error_simulation
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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
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
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

16
config/euroc_mav/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,16 @@
%YAML:1.0
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-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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

28
config/euroc_mav/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,28 @@
%YAML:1.0
cam0:
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]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
distortion_model: radtan
intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
resolution: [752, 480]
rostopic: /cam0/image_raw
cam1:
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]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05]
distortion_model: radtan
intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
resolution: [752, 480]
rostopic: /cam1/image_raw

122
config/kaist/estimator_config.yaml Normal file
View File

@@ -0,0 +1,122 @@
%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)
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_intrinsics: true
calib_cam_timeoffset: true
max_clones: 12
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"
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
zupt_chi2_multipler: 1 # 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
zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
init_window_time: 2.0
init_imu_thresh: 0.5 #0.5
init_max_disparity: 1.5
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]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 50
grid_x: 20
grid_y: 15
min_px_dist: 30
knn_ratio: 0.65
track_frequency: 21.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
fi_max_dist: 500
fi_max_baseline: 800
fi_max_cond_number: 20000
fi_triangulate_1d: false
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

23
config/kaist/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +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
# 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" />
model: calibrated
rostopic: /imu/data_raw
time_offset: 0.0
update_rate: 500.0

28
config/kaist/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,28 @@
%YAML:1.0 # need to specify the file type at the top!
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.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-5.6143027800000002e-02,1.3952563200000001e-01,-1.2155906999999999e-03,-9.7281389999999998e-04]
distortion_model: radtan
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]
- [-0.99988,-0.01305,-0.00788,-0.19706]
- [0.01317,-0.99980,-0.01499,-0.08271]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-5.4921981799999998e-02,1.4243657430000001e-01,7.5412299999999996e-05,-6.7560530000000001e-04]
distortion_model: radtan
intrinsics: [8.1378205539589999e+02,8.0852165574269998e+02,6.1386419539320002e+02,2.4941049348650000e+02]
resolution: [1280, 560]
rostopic: /stereo/right/image_raw

122
config/kaist_vio/estimator_config.yaml Normal file
View File

@@ -0,0 +1,122 @@
%YAML:1.0 # need to specify the file type at the top!
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)
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
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: 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_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
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.02
zupt_noise_multiplier: 10
zupt_max_disparity: 0.20 # 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: 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_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: 5.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-15
init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]
# ==================================================================
# ==================================================================
record_timing_information: false # if we want to record timing information of the method
record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame
# if we want to save the simulation state and its diagional covariance
# use this with rosrun ov_eval error_simulation
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 30
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 10.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
fi_max_dist: 10.0
fi_max_baseline: 200
fi_max_cond_number: 25000
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1.2
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1.2
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

20
config/kaist_vio/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,20 @@
%YAML:1.0 # need to specify the file type at the top!
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: 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
update_rate: 100.0

93
config/kaist_vio/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,93 @@
%YAML:1.0 # need to specify the file type at the top!
cam0:
T_cam_imu:
- - -0.04030123999740945
- -0.9989998755524683
- 0.01936643232049068
- 0.02103955032447366
- - 0.026311325355146964
- -0.020436499663524704
- -0.9994448777394171
- -0.038224929976612206
- - 0.9988410905708309
- -0.0397693113802049
- 0.027108627033059024
- -0.1363488241088845
- - 0.0
- 0.0
- 0.0
- 1.0
cam_overlaps:
- 1
camera_model: pinhole
distortion_coeffs:
- 0.006896928127777268
- -0.009144207062654397
- 0.000254113977103925
- 0.0021434982252719545
distortion_model: radtan
intrinsics:
- 380.9229090195708
- 380.29264802262736
- 324.68121181846755
- 224.6741321466431
resolution:
- 640
- 480
rostopic: /camera/infra1/image_rect_raw
timeshift_cam_imu: -0.029958533056650416
cam1:
T_cam_imu:
- - -0.03905752472566068
- -0.9990498568899562
- 0.019336318430946575
- -0.02909273113160158
- - 0.025035478432625047
- -0.020323396666370924
- -0.9994799569614147
- -0.03811090793611019
- - 0.99892328763622
- -0.03855311914877835
- 0.02580547271309183
- -0.13656684822705098
- - 0.0
- 0.0
- 0.0
- 1.0
T_cn_cnm1:
- - 0.9999992248836708
- 6.384241340452582e-05
- 0.0012434452955667624
- -0.049960282472300055
- - -6.225102643531651e-05
- 0.9999991790958949
- -0.0012798173093508036
- -5.920119010064575e-05
- - -0.001243525981443161
- 0.0012797389115975439
- 0.9999984079544582
- -0.00014316003395349448
- - 0.0
- 0.0
- 0.0
- 1.0
cam_overlaps:
- 0
camera_model: pinhole
distortion_coeffs:
- 0.007044055287844759
- -0.010251485722185347
- 0.0006674304399871926
- 0.001678899816379666
distortion_model: radtan
intrinsics:
- 380.95187095303424
- 380.3065956074995
- 324.0678433553536
- 225.9586983198407
resolution:
- 640
- 480
rostopic: /camera/infra2/image_rect_raw
timeshift_cam_imu: -0.030340187355085417

154
config/kaist_vio/pgeneva_ros_kaistvio.launch.old Normal file
View File

@@ -0,0 +1,154 @@
<launch>
<!-- mono or stereo and what ros bag to play -->
<arg name="max_cameras" default="2" />
<arg name="use_stereo" default="true" />
<arg name="bag_start" default="40" />
<arg name="dataset" default="rotation" /> <!-- circle, circle_fast, circle_head, square, square_fast, square_head -->
<arg name="bag" default="/media/patrick/RPNG\ FLASH\ 3/KAIST_VIO/$(arg dataset).bag" />
<arg name="bag_gt" default="$(find ov_data)/kaist_vio/$(arg dataset).txt" /> <!-- $(find ov_data)/kaist_vio/$(arg dataset).txt -->
<!-- imu starting thresholds -->
<arg name="init_window_time" default="1.5" />
<arg name="init_imu_thresh" default="0.1" />
<!-- saving trajectory path and timing information -->
<arg name="dosave" default="false" />
<arg name="dotime" default="false" />
<arg name="path_est" default="/tmp/traj_estimate.txt" />
<arg name="path_time" default="/tmp/traj_timing.txt" />
<!-- MASTER NODE! -->
<!-- <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
<node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">
<!-- bag topics -->
<param name="topic_imu" type="string" value="/mavros/imu/data" />
<param name="topic_camera0" type="string" value="/camera/infra1/image_rect_raw" />
<param name="topic_camera1" type="string" value="/camera/infra2/image_rect_raw" />
<!-- world/filter parameters -->
<param name="use_fej" type="bool" value="true" />
<param name="use_imuavg" type="bool" value="true" />
<param name="use_rk4int" type="bool" value="true" />
<param name="use_stereo" type="bool" value="$(arg use_stereo)" />
<param name="calib_cam_extrinsics" type="bool" value="false" /> <!-- degenerate motion causes this to fail (could use a smaller prior...) -->
<param name="calib_cam_intrinsics" type="bool" value="true" />
<param name="calib_cam_timeoffset" type="bool" value="true" />
<param name="calib_camimu_dt" type="double" value="-0.029958533056650416" />
<param name="max_clones" type="int" value="11" />
<param name="max_slam" type="int" value="50" />
<param name="max_slam_in_update" type="int" value="25" /> <!-- 25 seems to work well -->
<param name="max_msckf_in_update" type="int" value="40" />
<param name="max_cameras" type="int" value="$(arg max_cameras)" />
<param name="dt_slam_delay" type="double" value="1" />
<param name="init_window_time" type="double" value="$(arg init_window_time)" />
<param name="init_imu_thresh" type="double" value="$(arg init_imu_thresh)" />
<param name="gravity_mag" type="double" value="9.79858" /> <!-- 9.79858 -->
<param name="feat_rep_msckf" type="string" value="GLOBAL_3D" />
<param name="feat_rep_slam" type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
<param name="feat_rep_aruco" type="string" value="ANCHORED_FULL_INVERSE_DEPTH" />
<!-- zero velocity update parameters -->
<!-- inertial and disparity based detection (inertial is key for dynamic environments) -->
<param name="try_zupt" type="bool" value="true" />
<param name="zupt_chi2_multipler" type="double" value="0" /> <!-- set to 0 for only disp-based -->
<param name="zupt_max_velocity" type="double" value="0.1" />
<param name="zupt_noise_multiplier" type="double" value="50" />
<param name="zupt_max_disparity" type="double" value="0.2" /> <!-- set to 0 for only imu-based -->
<param name="zupt_only_at_beginning" type="bool" value="false" />
<!-- timing statistics recording -->
<param name="record_timing_information" type="bool" value="$(arg dotime)" />
<param name="record_timing_filepath" type="string" value="$(arg path_time)" />
<!-- tracker/extractor properties -->
<param name="use_klt" type="bool" value="true" />
<param name="num_pts" type="int" value="250" />
<param name="fast_threshold" type="int" value="15" />
<param name="grid_x" type="int" value="5" />
<param name="grid_y" type="int" value="3" />
<param name="min_px_dist" type="int" value="8" />
<param name="knn_ratio" type="double" value="0.70" />
<param name="downsample_cameras" type="bool" value="false" />
<param name="multi_threading" type="bool" value="true" />
<param name="histogram_method" type="string" value="HISTOGRAM" /> <!-- NONE, HISTOGRAM, CLAHE -->
<param name="fi_max_baseline" type="double" value="120" />
<param name="fi_max_cond_number" type="double" value="90000" />
<!-- aruco tag/mapping properties -->
<param name="use_aruco" type="bool" value="false" />
<param name="num_aruco" type="int" value="1024" />
<param name="downsize_aruco" type="bool" value="true" />
<!-- sensor noise values / update -->
<param name="up_msckf_sigma_px" type="double" value="1" />
<param name="up_msckf_chi2_multipler" type="double" value="1" />
<param name="up_slam_sigma_px" type="double" value="1" />
<param name="up_slam_chi2_multipler" type="double" value="1" />
<param name="up_aruco_sigma_px" type="double" value="1" />
<param name="up_aruco_chi2_multipler" type="double" value="1" />
<param name="gyroscope_noise_density" type="double" value="0.01" />
<param name="gyroscope_random_walk" type="double" value="0.0001" />
<param name="accelerometer_noise_density" type="double" value="0.1" />
<param name="accelerometer_random_walk" type="double" value="0.001" />
<!-- camera intrinsics -->
<rosparam param="cam0_wh">[640, 480]</rosparam>
<rosparam param="cam1_wh">[640, 480]</rosparam>
<param name="cam0_is_fisheye" type="bool" value="false" />
<param name="cam1_is_fisheye" type="bool" value="false" />
<rosparam param="cam0_k">[391.117,394.147,319.642,224.178]</rosparam>
<rosparam param="cam0_d">[0.00675,-0.00562,0.03121,-0.00828]</rosparam>
<rosparam param="cam1_k">[380.95187095303424, 380.3065956074995, 324.0678433553536, 225.9586983198407]</rosparam>
<rosparam param="cam1_d">[0.007044055287844759, -0.010251485722185347, 0.0006674304399871926, 0.001678899816379666]</rosparam>
<!-- camera extrinsics -->
<rosparam param="T_C0toI">
[
-0.0403012399974094,0.026311325355147,0.998841090570831,0.138044476707325,
-0.998999875552468,-0.0204364996635247,-0.0397693113802049,0.0148148255449128,
0.0193664323204907,-0.999444877739417,0.027108627033059,-0.0349149420753215,
0,0,0,1
]
</rosparam>
<rosparam param="T_C1toI">
[
-0.0390575247256606,0.025035478432625,0.99892328763622,0.136237639761255,
-0.999049856889956,-0.0203233966663709,-0.0385531191487784,-0.0351047099443363,
0.0193363184309466,-0.999479956961414,0.0258054727130918,-0.0340043702351212,
0,0,0,1
]
</rosparam>
</node>
<!-- play the dataset -->
<node pkg="rosbag" type="play" name="rosbag" args="-d 1 -s $(arg bag_start) $(arg bag)" required="true"/>
<!-- path viz of aligned gt -->
<node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
<param name="alignment_type" type="str" value="posyaw" />
<param name="path_gt" type="str" value="$(arg bag_gt)" />
</node>
<!-- record the trajectory if enabled -->
<group if="$(arg dosave)">
<node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
<param name="topic" type="str" value="/ov_msckf/poseimu" />
<param name="topic_type" type="str" value="PoseWithCovarianceStamped" />
<param name="output" type="str" value="$(arg path_est)" />
</node>
</group>
<!-- &lt;!&ndash; record the groundtruth if enabled &ndash;&gt;-->
<!-- <node name="gt" pkg="ov_eval" type="pose_to_file" output="screen" required="true">-->
<!-- <param name="topic" type="str" value="/pose_transformed" />-->
<!-- <param name="topic_type" type="str" value="PoseStamped" />-->
<!-- <param name="output" type="str" value="$(arg bag_gt)" />-->
<!-- </node>-->
</launch>

115
config/rpng_aruco/estimator_config.yaml Normal file
View File

@@ -0,0 +1,115 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints
max_cameras: 2
calib_cam_extrinsics: true
calib_cam_intrinsics: true
calib_cam_timeoffset: true
max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 50
dt_slam_delay: 2
gravity_mag: 9.81
feat_rep_msckf: "GLOBAL_3D"
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
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.1
zupt_noise_multiplier: 50
zupt_max_disparity: 0.5 # set to 0 for only imu-based
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_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-20
init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 150
fast_threshold: 30
grid_x: 20
grid_y: 20
min_px_dist: 30
knn_ratio: 0.85
track_frequency: 21.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: true
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 8
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

16
config/rpng_aruco/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,16 @@
%YAML:1.0
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-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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

28
config/rpng_aruco/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,28 @@
%YAML:1.0
cam0:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- [-0.99997806, -0.00600501, 0.0027968, 0.03847796]
- [0.00601449, -0.99997615, 0.00339343, -0.0045601]
- [0.00277635, 0.00341018, 0.99999033, 0.00418038]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.292031518680717,0.08753155838496009,0.0009568457669165753,2.3463489813256424e-05]
distortion_model: radtan
intrinsics: [470.0502737897896,468.7574814232544,405.80799445368035,245.2879780490104] #fu, fv, cu, cv
resolution: [752, 480]
rostopic: /cam0/image_raw
cam1:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- [-0.99984421, -0.00389232, 0.01721638, -0.07075565]
- [0.00394176, -0.9999882, 0.0028389, -0.00418534]
- [0.01720512, 0.00290632, 0.99984776, 0.00388861]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.2847596229068525,0.07908861097045865,0.0011071433505703875,0.0005094909873658998]
distortion_model: radtan
intrinsics: [472.98384780424163,471.9917417027018,382.1928744696835,268.2536666120421] #fu, fv, cu, cv
resolution: [752, 480]
rostopic: /cam1/image_raw

123
config/rpng_ironsides/estimator_config.yaml Normal file
View File

@@ -0,0 +1,123 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
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_intrinsics: true
calib_cam_timeoffset: true
max_clones: 12
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 50
dt_slam_delay: 1
gravity_mag: 9.80114
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
zupt_chi2_multipler: 1 # set to 0 for only disp-based
zupt_max_velocity: 0.5
zupt_noise_multiplier: 10
zupt_max_disparity: 0.4 # set to 0 for only imu-based
zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
#init_window_time: 1.0
init_window_time: 2.0
init_imu_thresh: 0.5
init_max_disparity: 1.5
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]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 20
knn_ratio: 0.65
track_frequency: 21.0
downsample_cameras: false
multi_threading: true
histogram_method: "CLAHE" # NONE, HISTOGRAM, CLAHE
fi_max_dist: 150
fi_max_baseline: 200
fi_max_cond_number: 20000
fi_triangulate_1d: false
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 2
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 2
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: true
mask0: "../../ov_data/masks/ironsides0.png" #relative to current file
mask1: "../../ov_data/masks/ironsides1.png" #relative to current file
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

16
config/rpng_ironsides/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,16 @@
%YAML:1.0
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: 0.0027052931930236323 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 1.3054568211204843e-04 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
gyroscope_noise_density: 1.1186830841306218e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
gyroscope_random_walk: 8.997530210630026e-07 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
model: calibrated
rostopic: /imu0
time_offset: 0.0
update_rate: 200.0

31
config/rpng_ironsides/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,31 @@
%YAML:1.0
cam0:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- [0.99992127, -0.0078594, 0.0097819, -0.05845078]
- [0.00784873, 0.99996856, 0.00112822, -0.00728728]
- [-0.00979046, -0.00105136, 0.99995152, 0.0623674]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.03149689493503132, 0.07696336480701078, -0.06608854732019281, 0.019667561645120218]
distortion_model: equidistant
intrinsics: [276.4850207717928, 278.0310503180516, 314.5836189313042, 240.16980920673427] #fu, fv, cu, cv
resolution: [640, 480]
rostopic: /cam0/image_raw
timeshift_cam_imu: 0.00621
cam1:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- [0.99995933, 0.00327998, 0.00840069, 0.00793529]
- [-0.00328309, 0.99999455, 0.000356, -0.00716413]
- [-0.00839948, -0.00038357, 0.99996465, 0.06245421]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.02998039058251529, 0.07202819722706337, -0.06178718820631651, 0.017655045017816777]
distortion_model: equidistant
intrinsics: [277.960323846132, 279.4348778432714, 322.404194404853, 236.72685252691352] #fu, fv, cu, cv
resolution: [640, 480]
rostopic: /cam1/image_raw
timeshift_cam_imu: 0.00621

128
config/rpng_sim/estimator_config.yaml Normal file
View File

@@ -0,0 +1,128 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints
max_cameras: 2
calib_cam_extrinsics: true
calib_cam_intrinsics: true
calib_cam_timeoffset: true
max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 10
dt_slam_delay: 2
gravity_mag: 9.81
feat_rep_msckf: "GLOBAL_3D"
feat_rep_slam: "GLOBAL_3D"
feat_rep_aruco: "GLOBAL_3D"
# zero velocity update parameters we can use
# we support either IMU-based or disparity detection.
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: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: true
# ==================================================================
# ==================================================================
init_window_time: 2.0
init_imu_thresh: 1.0
init_max_disparity: 1.5
init_max_features: 15
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.5 # 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: 15.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-15 # 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
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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
track_frequency: 21.0
downsample_cameras: false
multi_threading: false
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"
# ==================================================================
# ==================================================================
sim_seed_state_init: 0
sim_seed_preturb: 0
sim_seed_measurements: 0
sim_do_perturbation: false
sim_traj_path: "src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt"
sim_distance_threshold: 1.2
sim_freq_cam: 10
sim_freq_imu: 400
sim_min_feature_gen_dist: 5.0
sim_max_feature_gen_dist: 7.0

16
config/rpng_sim/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,16 @@
%YAML:1.0
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-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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

28
config/rpng_sim/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,28 @@
%YAML:1.0
cam0:
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]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
distortion_model: radtan
intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
resolution: [752, 480]
rostopic: /cam0/image_raw
cam1:
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]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05]
distortion_model: radtan
intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
resolution: [752, 480]
rostopic: /cam1/image_raw

117
config/tum_vi/estimator_config.yaml Normal file
View File

@@ -0,0 +1,117 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints
max_cameras: 2
calib_cam_extrinsics: true
calib_cam_intrinsics: true
calib_cam_timeoffset: true
max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 40
dt_slam_delay: 2
gravity_mag: 9.80766
feat_rep_msckf: "GLOBAL_3D"
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: false
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.1
zupt_noise_multiplier: 50
zupt_max_disparity: 2.0 # set to 0 for only imu-based
zupt_only_at_beginning: true
# ==================================================================
# ==================================================================
init_window_time: 2.0
init_imu_thresh: 0.45 # room1-5:0.45, room6:0.25
init_max_disparity: 5.0
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: 20.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-20
init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 150
fast_threshold: 20
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.65
track_frequency: 21.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: true
mask0: "../../ov_data/masks/tumvi0.png" #relative to current file
mask1: "../../ov_data/masks/tumvi1.png" #relative to current file
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

16
config/tum_vi/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,16 @@
%YAML:1.0 # need to specify the file type at the top!
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: 0.0028 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 0.00086 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
gyroscope_noise_density: 0.00016 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
gyroscope_random_walk: 2.2e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
model: calibrated
rostopic: /imu0
time_offset: 0.0
update_rate: 200.0

30
config/tum_vi/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,30 @@
%YAML:1.0 # need to specify the file type at the top!
cam0:
T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera p_IinC
- [-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392]
- [0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084]
- [-0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182]
distortion_model: equidistant
intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
resolution: [512, 512]
rostopic: /cam0/image_raw
cam1:
T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera p_IinC
- [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734]
- [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924]
- [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751]
- [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_model: equidistant
intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
resolution: [512, 512]
rostopic: /cam1/image_raw

116
config/uzhfpv_indoor/estimator_config.yaml Normal file
View File

@@ -0,0 +1,116 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
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_intrinsics: true
calib_cam_timeoffset: true
max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 30
dt_slam_delay: 2
gravity_mag: 9.81
feat_rep_msckf: "GLOBAL_3D"
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
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.5
zupt_noise_multiplier: 20
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
init_window_time: 2.0
init_imu_thresh: 0.0
init_max_disparity: 1.5
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: 20.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-20
init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 100
fast_threshold: 65
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 2
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 2
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

20
config/uzhfpv_indoor/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,20 @@
%YAML:1.0 # need to specify the file type at the top!
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-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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 )
# accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
# accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
# gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
# gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
model: calibrated
rostopic: /snappy_imu
time_offset: 0.0
update_rate: 200.0

37
config/uzhfpv_indoor/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,37 @@
%YAML:1.0 # need to specify the file type at the top!
cam0:
T_cam_imu:
- [-0.028228787368606456, -0.999601488301944, 1.2175294828553618e-05, 0.02172388268966517]
- [0.014401251861751119, -0.00041887083271471837, -0.9998962088597202, -6.605455433829172e-05]
- [0.999497743623523, -0.028225682131089447, 0.014407337010089172, -0.00048817563004522853]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.013721808247486035, 0.020727425669427896, -0.012786476702685545,
0.0025242267320687625]
distortion_model: equidistant
intrinsics: [278.66723066149086, 278.48991409740296, 319.75221200593535, 241.96858910358173]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_l
timeshift_cam_imu: -0.016684572091862235
cam1:
T_cam_imu:
- [-0.011823057800830705, -0.9998701444077991, -0.010950325390841398, -0.057904961033265645]
- [0.011552991631909482, 0.01081376681432078, -0.9998747875767439, 0.00043766687615362694]
- [0.9998633625093938, -0.011948086424720228, 0.011423639621249038, -0.00039944945687402214]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9998053017199788, 0.011197738450911484, 0.01624713224548414, -0.07961594300469246]
- [-0.011147758116324, 0.9999328574031386, -0.0031635699090552883, 0.0007443452072558462]
- [-0.016281466199246444, 0.00298183486707869, 0.9998630018753686, 0.0004425529195268342]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.008456929295619607, 0.011407590938612062, -0.006951788325762078,
0.0015368127092821786]
distortion_model: equidistant
intrinsics: [277.61640629770613, 277.63749695723294, 314.8944703346039, 236.04310050462587]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_r
timeshift_cam_imu: -0.016591431247074982

116
config/uzhfpv_indoor_45/estimator_config.yaml Normal file
View File

@@ -0,0 +1,116 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
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_intrinsics: true
calib_cam_timeoffset: true
max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 30
dt_slam_delay: 2
gravity_mag: 9.81
feat_rep_msckf: "GLOBAL_3D"
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
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.5
zupt_noise_multiplier: 20
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
init_window_time: 2.0
init_imu_thresh: 0.0
init_max_disparity: 1.5
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: 20.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-20
init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 100
fast_threshold: 65
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 2
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 2
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

20
config/uzhfpv_indoor_45/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,20 @@
%YAML:1.0 # need to specify the file type at the top!
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-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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 )
# accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
# accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
# gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
# gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
model: calibrated
rostopic: /snappy_imu
time_offset: 0.0
update_rate: 200.0

37
config/uzhfpv_indoor_45/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,37 @@
%YAML:1.0 # need to specify the file type at the top!
cam0:
T_cam_imu:
- [-0.027256691772188965, -0.9996260641688061, 0.0021919370477445077, 0.02422852666805565]
- [-0.7139206120417471, 0.017931469899155242, -0.6999970157716363, 0.008974432843748055]
- [0.6996959571525168, -0.020644471939022302, -0.714142404092339, -0.000638971731537894]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-6.545154718304953e-06, -0.010379525898159981, 0.014935312423953146,
-0.005639061406567785]
distortion_model: equidistant
intrinsics: [275.46015578667294, 274.9948095922592, 315.958384100568, 242.7123497822731]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_l
timeshift_cam_imu: -0.01484888826656275
cam1:
T_cam_imu:
- [-0.01749277298389329, -0.9997914625864506, -0.010537278233961556, -0.05569997768397372]
- [-0.7090991957246053, 0.019835234209851005, -0.7048296915614142, 0.00884826894411553]
- [0.7048917175822481, -0.004857450265962848, -0.7092982952614942, -0.0019997713120269607]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9998713028057842, 0.0019367839962957476, 0.015925661468614183, -0.07993259169905036]
- [-0.0020450612045710957, 0.9999748874354272, 0.0067854420026733295, -7.205400304978925e-05]
- [-0.01591211959893364, -0.006817137687752675, 0.9998501543149417, -0.0009141881692325364]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.012138050918285051, 0.02244029339184358, -0.013753165428754275,
0.002725090438517269]
distortion_model: equidistant
intrinsics: [274.4628309070672, 273.9261674470783, 315.93654481793794, 235.779167375461]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_r
timeshift_cam_imu: -0.014950736007814259

118
config/uzhfpv_outdoor/estimator_config.yaml Normal file
View File

@@ -0,0 +1,118 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
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_intrinsics: true
calib_cam_timeoffset: true
max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 30
dt_slam_delay: 2
gravity_mag: 9.81
feat_rep_msckf: "GLOBAL_3D"
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
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.5
zupt_noise_multiplier: 20
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
init_window_time: 2.0
init_imu_thresh: 0.0
init_max_disparity: 1.5
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: 20.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-20
init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 100
fast_threshold: 65
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 2
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 2
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: true
mask0: "../../ov_data/masks/uzhfpv_outdoor_mask0.png" #relative to current file
mask1: "../../ov_data/masks/uzhfpv_outdoor_mask1.png" #relative to current file
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

20
config/uzhfpv_outdoor/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,20 @@
%YAML:1.0 # need to specify the file type at the top!
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-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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 )
# accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
# accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
# gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
# gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
model: calibrated
rostopic: /snappy_imu
time_offset: 0.0
update_rate: 200.0

37
config/uzhfpv_outdoor/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,37 @@
%YAML:1.0 # need to specify the file type at the top!
cam0:
T_cam_imu:
- [-0.03179778293757218, -0.9994933985910031, -0.001359107523862424, 0.021115239798621798]
- [0.012827844120885779, 0.0009515801497960164, -0.9999172670328424, -0.0008992998316121829]
- [0.9994120008362244, -0.03181258663210035, 0.012791087377928778, -0.009491094814035777]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.005719912631104124, 0.004742449009601135, 0.0012060658036136048,
-0.001580292679344826]
distortion_model: equidistant
intrinsics: [277.4786896484645, 277.42548548840034, 320.1052053576385, 242.10083077857894]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_l
timeshift_cam_imu: -0.007999243205055177
cam1:
T_cam_imu:
- [-0.011450159873389598, -0.9998746482793399, -0.010935335712288774, -0.05828448770624624]
- [0.009171247533644289, 0.010830579777447058, -0.9998992883087583, -0.0002362068202437068]
- [0.999892385238307, -0.01154929737910465, 0.009046086032012068, -0.00947464531803495]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9997470623689986, 0.009836089265916417, 0.020225296846065624, -0.07919358086270675]
- [-0.00975774768296796, 0.9999445171722606, -0.0039684930755682956, 0.000831414953842084]
- [-0.020263209141547188, 0.0037701359508940783, 0.9997875716521978, 0.00044568632114983057]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.009025009906076716, 0.009967427035376123, -0.0029538969814842117,
-0.0003503551771748748]
distortion_model: equidistant
intrinsics: [276.78679780974477, 276.79332134030807, 314.2862327340746, 236.51313088043128]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_r
timeshift_cam_imu: -0.007983859928063504

116
config/uzhfpv_outdoor_45/estimator_config.yaml Normal file
View File

@@ -0,0 +1,116 @@
%YAML:1.0 # need to specify the file type at the top!
verbosity: "INFO" # 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)
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_intrinsics: true
calib_cam_timeoffset: true
max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 30
dt_slam_delay: 2
gravity_mag: 9.81
feat_rep_msckf: "GLOBAL_3D"
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
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.5
zupt_noise_multiplier: 20
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
init_window_time: 2.0
init_imu_thresh: 0.0
init_max_disparity: 1.5
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: 20.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-20
init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]
# ==================================================================
# ==================================================================
record_timing_information: false
record_timing_filepath: "/tmp/traj_timing.txt"
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
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: 100
fast_threshold: 65
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
multi_threading: true
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true
# ==================================================================
# ==================================================================
# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 2
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 2
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1
# masks for our images
use_mask: false
# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

20
config/uzhfpv_outdoor_45/kalibr_imu_chain.yaml Normal file
View File

@@ -0,0 +1,20 @@
%YAML:1.0 # need to specify the file type at the top!
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-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
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 )
# accelerometer_noise_density: 0.1 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
# accelerometer_random_walk: 0.002 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
# gyroscope_noise_density: 0.05 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
# gyroscope_random_walk: 4.0e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
model: calibrated
rostopic: /snappy_imu
time_offset: 0.0
update_rate: 200.0

37
config/uzhfpv_outdoor_45/kalibr_imucam_chain.yaml Normal file
View File

@@ -0,0 +1,37 @@
%YAML:1.0 # need to specify the file type at the top!
cam0:
T_cam_imu:
- [-0.024041523213909927, -0.9996640790624955, 0.009681642096550924, 0.02023430742078562]
- [-0.7184527320882621, 0.010542697330412382, -0.6954958830129113, 0.008311861463499775]
- [0.6951601807615744, -0.023676582632001453, -0.7184648512755534, -0.026628438421085154]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.017811595366268803, 0.04897078939103475, -0.041363300782847834,
0.011440891936886532]
distortion_model: equidistant
intrinsics: [275.3385453506587, 275.0852058534152, 315.7697752181792, 233.72625444124952]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_l
timeshift_cam_imu: -0.008637511810764048
cam1:
T_cam_imu:
- [-0.004527750456351745, -0.9999560749011355, -0.008206567133703047, -0.05986676424716047]
- [-0.7208238256076104, 0.008951751262681593, -0.6930605158178762, 0.008989928313050033]
- [0.6931035362139012, 0.0027774840496477826, -0.7208326946456712, -0.026595921269512067]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9996495696908625, -0.0015816259006504233, 0.026424160845286468, -0.07937720055807065]
- [0.0016709946890799124, 0.9999929578963755, -0.0033603473630593734, 0.0005548331594719445]
- [-0.026418659951183095, 0.0034033244279300045, 0.9996451729434878, 0.0005293439870858398]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [0.027860492621377443, -0.027723581962855317, 0.0375199775145906,
-0.018152613898714216]
distortion_model: equidistant
intrinsics: [273.2895238376505, 273.35830490745764, 314.60557378520133, 251.0359907029701]
resolution: [640, 480]
rostopic: /snappy_cam/stereo_r
timeshift_cam_imu: -0.008613446015312496