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

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>