前言
周末休息了两天，接着做上周五那个VIRAL数据集没有运行成功的工作。现在的最新OpenVINS需要重新写配置文件，不像之前那样都写在launch里，因此需要根据数据集情况配置好estimator_config.yaml还有两个标定参数文件。
VIRAL数据集
VIRAL数据集包含雷达、相机、IMU、UWB四种数据，是南洋理工大学在22年发布的。
官网地址：https://ntu-aris.github.io/ntu_viral_dataset/
适配VIRAL的OpenVINS（旧版）：https://github.com/brytsknguyen/open_vins.git
VIRAL数据集本身作者对一些常用VIO开源代码做了适配修改，其中就包括OpenVINS，但是这个是更新之前的OpenVINS，现在的使用方式配置和之前有所不同。我刚开始从Euroc的数据集配置改动，只是改VIRAL以前OpenVINS配置的参数，初始化跑不通，如下图所示。
这是VIRAL适配的openvins的配置情况，是通过launch进行配置的。
<launch>
<param name="/use_sim_time" value="true" />
<arg name="publish_clock" default="--clock"/>
<!-- NTU VIRAL dataset -->
<!-- EEE -->
<arg name="bag_file" default="/home/merlincs/workspace/dataset/VIRAL/eee_01/eee_01.bag"/>
<!-- MASTER NODE! -->
<node name="run_serial_msckf" pkg="ov_msckf" type="run_serial_msckf" output="screen" clear_params="true" required="true">
<!-- bag topics -->
<param name="topic_imu" type="string" value="/imu/imu" />
<param name="topic_camera0" type="string" value="/right/image_raw" />
<param name="topic_camera1" type="string" value="/left/image_raw" />
<rosparam param="stereo_pairs">[0,1]</rosparam>
<!-- bag parameters -->
<param name="path_bag" type="string" value="$(arg bag_file)" />
<!-- <param name="path_gt" type="string" value="$(find ov_data)/euroc_mav/V1_01_easy.csv" /> -->
<!-- <param name="bag_start" type="double" value="0" /> -->
<!-- <param name="bag_durr" type="int" value="-1" /> -->
<!-- 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="true" />
<param name="calib_cam_extrinsics" type="bool" value="true" />
<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.0" />
<param name="max_clones" type="int" value="11" />
<param name="max_slam" type="int" value="75" />
<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="2" />
<param name="dt_slam_delay" type="double" value="3" />
<param name="init_window_time" type="double" value="0.75" />
<param name="init_imu_thresh" type="double" value="0.25" />
<rosparam param="gravity">[0.0,0.0,9.81]</rosparam>
<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 -->
<param name="try_zupt" type="bool" value="false" />
<param name="zupt_chi2_multipler" type="int" value="2" />
<param name="zupt_max_velocity" type="double" value="0.3" />
<param name="zupt_noise_multiplier" type="double" value="50" />
<!-- timing statistics recording -->
<param name="record_timing_information" type="bool" value="false" />
<param name="record_timing_filepath" type="string" value="/tmp/timing_stereo.txt" />
<!-- 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="5" />
<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" />
<!-- 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="5.0e-3" />
<param name="gyroscope_random_walk" type="double" value="3.0e-6" />
<param name="accelerometer_noise_density" type="double" value="6.0e-2" />
<param name="accelerometer_random_walk" type="double" value="8.0e-5" />
<!-- camera intrinsics -->
<rosparam param="cam0_wh">[752, 480]</rosparam>
<rosparam param="cam1_wh">[752, 480]</rosparam>
<param name="cam0_is_fisheye" type="bool" value="false" />
<param name="cam1_is_fisheye" type="bool" value="false" />
<rosparam param="cam0_k">[4.313364265799752e+02, 4.327527965378035e+02, 3.548956286992647e+02, 2.325508916495161e+02]</rosparam>
<rosparam param="cam0_d">[-0.300267420221178, 0.090544063693053, 3.330220891093334e-05, 8.989607188457415e-05]</rosparam>
<rosparam param="cam1_k">[4.250258563372763e+02, 4.267976260903337e+02, 3.860151866550880e+02, 2.419130336743440e+02]</rosparam>
<rosparam param="cam1_d">[-0.288105327549552, 0.074578284234601, 7.784489598138802e-04, -2.277853975035461e-04]</rosparam>
<!-- camera extrinsics -->
<rosparam param="T_C0toI">
[
-0.01916508, -0.01496218, 0.99970437, 0.00519443,
0.99974371, 0.01176483, 0.01934191, 0.1347802,
-0.01205075, 0.99981884, 0.01473287, 0.01465067,
0.00000000, 0.00000000, 0.00000000, 1.00000000
]
</rosparam>
<rosparam param="T_C1toI">
[
0.02183084, -0.01312053, 0.99967558, 0.00552943,
0.99975965, 0.00230088, -0.02180248, -0.12431302,
-0.00201407, 0.99991127, 0.01316761, 0.01614686,
0.00000000, 0.00000000, 0.00000000, 1.00000000
]
</rosparam>
</node>
<node pkg="rviz" type="rviz" name="ov_msckf_rviz" respawn="true" output="log"
args="-d $(find ov_msckf)/launch/ntuviral.rviz" />
<!-- <arg name="autorun" default="false"/>
<node required="$(arg autorun)" pkg="rosbag" type="play" name="rosbag_play"
args="$(arg publish_clock) $(arg bag_file) -r 1"/> -->
</launch>
对应把上面参数写入新建的config/viral中三个配置文件后跑不通：
主要原因是因为静态初始化运动检测的原因，具体原理我也还不是很清楚，下一次博客对于初始化这块做详细的学习。因此除了抄viral适配openvins中的配置外，还需要对配置文件进行一些改动，下面介绍一下配置文件各个参数含义。
配置文件详解
config文件夹内有三个配置文件：
estimator_config.yaml，kalibr_imucam_chain.yaml，kalibr_imu_chain.yaml。
第一个是针对不同数据集对估计器的配置，第二个第三个是相机和IMU的标定参数。
下面是针对viral数据集进行修改过的配置文件。（目前还只是对eee01.bag这一个数据包初始化有效）
1、estimator_config.yaml
%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)
integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs
max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized
calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix)
calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated
max_clones: 11 # how many clones in the sliding window
max_slam: 75 # 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: 3 # 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_FULL_INVERSE_DEPTH"
feat_rep_aruco: "ANCHORED_FULL_INVERSE_DEPTH"
# zero velocity update parameters we can use
# we support either IMU-based or disparity detection.
try_zupt: false
zupt_chi2_multipler: 2 # set to 0 for only disp-based
zupt_max_velocity: 0.3
zupt_noise_multiplier: 50
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
init_window_time: 0.75 # how many seconds to collect initialization information
init_imu_thresh: 0.25 # threshold for variance of the accelerometer to detect a "jerk" in motion
init_max_disparity: 1.0 # max disparity to consider the platform stationary (dependent on resolution)
init_max_features: 20 # 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
# ==================================================================
# ==================================================================
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 # if true we will use KLT, otherwise use a ORB descriptor + robust matching
num_pts: 250 # number of points (per camera) we will extract and try to track
fast_threshold: 15 # threshold for fast extraction (warning: lower threshs can be expensive)
grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking)
grid_y: 3 # extraction sub-grid count for vertical direction (uniform tracking)
min_px_dist: 5 # 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: 11.0 # frequency we will perform feature tracking at (in frames per second / hertz)
downsample_cameras: false # will downsample image in half if true
num_opencv_threads: -1 # -1: auto, 0-1: serial, >1: number of threads
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"
2、kalibr_imucam_chain.yaml
%YAML:1.0
cam0:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- [-0.01916508, -0.01496218, 0.99970437, 0.00519443]
- [0.99974371, 0.01176483, 0.01934191, 0.1347802]
- [-0.01205075, 0.99981884, 0.01473287, 0.01465067]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole#相机模型
distortion_coeffs: [-0.300267420221178, 0.090544063693053, 3.330220891093334e-05, 8.989607188457415e-05]#畸变参数
distortion_model: radtan#畸变模型
intrinsics: [4.313364265799752e+02, 4.327527965378035e+02, 3.548956286992647e+02, 2.325508916495161e+02] #fu, fv, cu, cv
resolution: [752, 480]#分辨率
rostopic: /right/image_raw
cam1:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- [0.02183084, -0.01312053, 0.99967558, 0.00552943]
- [0.99975965, 0.00230088, -0.02180248, -0.12431302]
- [-0.00201407, 0.99991127, 0.01316761, 0.01614686]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.288105327549552, 0.074578284234601, 7.784489598138802e-04, -2.277853975035461e-04]
distortion_model: radtan
intrinsics: [4.250258563372763e+02, 4.267976260903337e+02, 3.860151866550880e+02, 2.419130336743440e+02] #fu, fv, cu, cv
resolution: [752, 480]
rostopic: /left/image_raw
3、kalibr_imu_chain.yaml
%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: 6.0e-2 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 8.0e-5 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )
gyroscope_noise_density: 5.0e-3 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
gyroscope_random_walk: 3.0e-6 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
rostopic: /imu/imu
time_offset: 0.0
update_rate: 385.0#IMU更新频率
# three different modes supported:
# "calibrated" (same as "kalibr"), "kalibr", "rpng"
model: "kalibr"
# how to get from Kalibr imu.yaml result file:
# - Tw is imu0:gyroscopes:M:
# - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
# - Ta is imu0:accelerometers:M:
# - R_IMUtoACC not used by Kalibr
# - Tg is imu0:gyroscopes:A:
Tw:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
R_IMUtoGYRO:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
Ta:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
R_IMUtoACC:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
Tg:
- [ 0.0, 0.0, 0.0 ]
- [ 0.0, 0.0, 0.0 ]
- [ 0.0, 0.0, 0.0 ]
实验结果
按照上面进行配置文件修改，然后运行如下命令
#第一个终端
roscore
#第二个终端
source devel/setup.bash
roslaunch ov_msckf subscribe.launch config:=viral
#第三个终端
rviz
#然后导入配置ntuviral.rviz（从viral适配的openvins中下载，在ov_msckf/launch中）
#数据文件夹下打开第四个终端
rosbag play eee_01.bag
运行结果如图所示
现在还只能在eee01.bag这一个数据包初始化能跑通，同样的配置跑eee02.bag就不行，初始化这块还是要明白原理，才能够更好地进行配置。接下来重点学习一下OpenVINS的初始化原理，看看怎么配置静态初始化和动态初始化（新版本开源的新功能应该很好用）。
