前言
周末休息了兩天,接著做上周五那個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的初始化原理,看看怎么配置靜態初始化和動態初始化(新版本開源的新功能應該很好用)。