OpenVINS学习2——VIRAL数据集eee01.bag运行
前言
周末休息了两天，接着做上周五那个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, SILENTuse_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 calibratedmax_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 locationfeat_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 initinit_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 inversioninit_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.0cam0: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, cvresolution: [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: pinholedistortion_coeffs: [-0.288105327549552, 0.074578284234601, 7.784489598138802e-04, -2.277853975035461e-04]distortion_model: radtanintrinsics: [4.250258563372763e+02, 4.267976260903337e+02, 3.860151866550880e+02, 2.419130336743440e+02] #fu, fv, cu, cvresolution: [752, 480]rostopic: /left/image_raw
3、kalibr_imu_chain.yaml
%YAML:1.0imu0: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/imutime_offset: 0.0update_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的初始化原理，看看怎么配置静态初始化和动态初始化（新版本开源的新功能应该很好用）。