转自 https://www.jianshu.com/p/194d6c9ef9a4
不确定该文章有没有对IMU和相机的时间做同步!
1. 使用imu_utils
工具包标定IMU。
这里有个坑,imu_utils
依赖code_utils
,但不要同时放到src下进行编译。 先编译code_utils
,再编译imu_utils
。
此外,编译code_utils
会报错,
code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory
此时在code_utils
下面找到sumpixel_test.cpp
,修改#include "backward.hpp"
为#include “code_utils/backward.hpp”
,再编译。
1.1 打开T265并发布imu话题
修改rs_t265.launch如下:
<arg name="unite_imu_method" default="linear_interpolation"/>
修改后才能发布imu话题,然后开启:
roslaunch realsense2_camera rs_t265.launch
1.2 新建t265_imu.launch
在~/catkin_ws/src/imu_utils/launch中新建t265_imu.launch,如下: (记得把#号后的内容删除)
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/camera/imu"/>
<param name="imu_name" type="string" value= "BMI055"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "60"/>
<param name="max_cluster" type="int" value= "200"/>
</node>
</launch>
1.3 运行imu校准程序
roslaunch imu_utils t265_imu.launch
等60分钟后就会生成结果,imu_utils/data
下的BMI055_imu_param.yaml
。该文件给出了加速度计和陀螺仪三轴的noise_density(后缀n)和random_walk(后缀w),同时计算出了平均值,后面IMU+摄像头联合标定的时候需要这些均值。
2. 相机标定
下载官方给的april_6x6_80x80cm_A0.pdf
或者其它标定文件。打印或者在屏幕显示,量尺寸后新建或者修改apriltags.yaml
。我的如下:
target_type: 'aprilgrid'
tagCols: 6
tagRows: 6
tagSize: 0.03
tagSpacing: 0.333
开启T265
roslaunch realsense2_camera rs_t265.launch
降低图像话题频率
rosrun topic_tools throttle messages /camera/fisheye1/image_raw 4.0 /fisheye1
rosrun topic_tools throttle messages /camera/fisheye2/image_raw 4.0 /fisheye2
然后录制,注意相机要缓慢移动,同时相机视野要充分看到apriltags
rosbag record -O cameras_calibration /fisheye1 /fisheye2
调用kalibr的算法计算各个摄像头的内参和外参
使用默认可能会报错
Using the default setup in the initial run leads to an error of Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
这是由于两个相机之间不同步造成的
解决方法:在末尾加上
--approx-sync 0.04
kalibr_calibrate_cameras --target ../apriltags.yaml --bag ./bag/cameras_calibration.bag --bag-from-to 5 80 --models omni-radtan omni-radtan --topics /fisheye1 /fisheye2 --approx-sync 0.04
omni-radtan为相机模型,可参考[3]。
最终输出一个pdf和txt文件,有内外参数据。
3. Camera-IMU联合标定
新建imu.yaml
,将之前IMU标定生成的yaml文件复制过来并改为kalibr提供的格式,如下:
accelerometer_noise_density: 1.85e-03
accelerometer_random_walk: 2.548e-05
gyroscope_noise_density: 1.094e-02
gyroscope_random_walk: 5.897e-04
rostopic: /imu
update_rate: 200.0
修改rs_t265.launch,一个是保持IMU和图像信息同步,另一个要确保输出IMU数据。
<arg name="enable_sync" default="true"/>,
<arg name="unite_imu_method" default="copy"/>
kalibr推荐IMU 200Hz,图像20Hz,参考上面用topic_tools throttle限制频率,然后录制bag。之后调用kalibr的算法计算IMU和camera外参。
kalibr_calibrate_imu_camera --target ../apriltags.yaml --cam ../camchain-stereo_calibration.yaml --imu ../imu.yaml --bag ../imu_cameras_calibration.bag --bag-from-to 10 100 --max-iter 30 --show-extraction
最终输出results-imucam-stereo_calibration.txt和camchain-imucam-stereo_calibration.yaml
。
4. 运行VINS-Fusion
在config中新建fisheye1.yaml,fisheye2.yaml,stereo_imu.yaml
将上面生成的联合标定结果参照其它相机config写入。我的如下:
fisheye1.yaml
%YAML:1.0
---
model_type: MEI
camera_name: camera
image_width: 848
image_height: 800
mirror_parameters:
xi: 1.6943561
distortion_parameters:
k1: -0.1075293
k2: 0.6081762
p1: 0.0029581
p2: 0.0020715
projection_parameters:
gamma1: 774.927
gamma2: 773.762
u0: 420.086
v0: 402.516
stereo_imu.yaml
%YAML:1.0
imu: 1
num_of_cam: 2
imu_topic: "/camera/imu"
image0_topic: "/camera/fisheye1/image_raw"
image1_topic: "/camera/fisheye2/image_raw"
output_path: "/home/zhang/Downloads/output/"
cam0_calib: "fisheye1.yaml"
cam1_calib: "fisheye2.yaml"
image_width: 848
image_height: 800
estimate_extrinsic: 1
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [-0.9996934517722752, -0.017627605360341365, 0.017385914209325233, 0.005754793009546214,
0.01763901821691402, -0.999844293739213, 0.0005033025707699151, 0.004283178521816982,
0.017374335094538958, 0.0008098187417148282, 0.999848726895038, -0.005638553131443425,
0., 0., 0., 1.]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [-0.999768258980969, -0.01371448358867344, 0.016593410561375914, -0.06100400373236911,
0.013686981433494675, -0.9999047625489491, -0.001769850606391636, 0.004859511023863885,
0.016616102834345566, -0.0015423267571366712, 0.9998607534825902, -0.0022157241622004077,
0., 0., 0., 1. ]
multiple_thread: 1
max_cnt: 150
min_dist: 30
freq: 10
F_threshold: 1.0
show_track: 0
flow_back: 1
max_solver_time: 0.04
max_num_iterations: 8
keyframe_parallax: 10.0
acc_n: 1.09387e-02
gyr_n: 1.8491e-03
acc_w: 5.8973e-04
gyr_w: 2.5482e-05
g_norm: 9.805
estimate_td: 1
td: 0.004
load_previous_pose_graph: 0
pose_graph_save_path: "/home/zhang/Downloads/output/pose_graph/"
save_image: 0
然后参考官方的运行步骤[4],效果如下(红色为回环轨迹):
感谢以下相关文章及作者给的帮助
[1] RealSenseD435i - imu + camera标定
[2] 用imu_utils标定IMU,之后用于kalibr中相机和IMU的联合标定
[3] kalibr camera models
[4] VINS-Fusion
[5] Cheng Wei’s Blog
作者:名字都被取完了妈个鸡
链接:https://www.jianshu.com/p/194d6c9ef9a4
来源:简书
著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)