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, 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的初始化原理，看看怎么配置静态初始化和动态初始化（新版本开源的新功能应该很好用）。