bmi055 标定_Realsense T265标定及运行VINS--kalibr和imu_utils

因项目需要用T265跑VINS之类的算法，但网上关于realsense T265的资料较少，这两天爬了很多坑，算是把T265的标定做好了。同时也跑了VINS-Fusion(图在最下)，效果算是可以吧(吗)。

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如下:

修改后才能发布imu话题，然后开启:

roslaunch realsense2_camera rs_t265.launch

1.2 新建t265_imu.launch

在~/catkin_ws/src/imu_utils/launch中新建t265_imu.launch,如下: (记得把#号后的内容删除)

#IMU在ROS中发布的话题

#IMU型号

#运行的时长(min)，60-120min

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' #gridtype

tagCols: 6 #number of apriltags

tagRows: 6 #number of apriltags

tagSize: 0.03 #size of apriltag, edge to edge [m]

tagSpacing: 0.333 #ratio of space between tags to tagSize

#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

开启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提供的格式，如下:

#Accelerometers

accelerometer_noise_density: 1.85e-03 #Noise density (continuous-time)

accelerometer_random_walk: 2.548e-05 #Bias random walk

#Gyroscopes

gyroscope_noise_density: 1.094e-02 #Noise density (continuous-time)

gyroscope_random_walk: 5.897e-04 #Bias random walk

rostopic: /imu #the IMU ROS topic

update_rate: 200.0 #Hz (for discretization of the values above)

修改rs_t265.launch，一个是保持IMU和图像信息同步，另一个要确保输出IMU数据。

，

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

#common parameters

#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;

imu: 1

num_of_cam: 2

#imu_topic: "/imu0"

imu_topic: "/camera/imu"

#image0_topic: "/cam0/image_raw"

#image1_topic: "/cam1/image_raw"

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

# Extrinsic parameter between IMU and Camera.

estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.

# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.

body_T_cam0: !!opencv-matrix #cam0 coordinate under body coordinate

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 support

multiple_thread: 1

#feature traker paprameters

max_cnt: 150 # max feature number in feature tracking

min_dist: 30 # min distance between two features

freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image

F_threshold: 1.0 # ransac threshold (pixel)

show_track: 0 # publish tracking image as topic

flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters

max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time

max_num_iterations: 8 # max solver itrations, to guarantee real time

keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters The more accurate parameters you provide, the better performance

acc_n: 1.09387e-02 # accelerometer measurement noise standard deviation. #0.2 0.04

gyr_n: 1.8491e-03 # gyroscope measurement noise standard deviation. #0.05 0.004

acc_w: 5.8973e-04 # accelerometer bias random work noise standard deviation. #0.002

gyr_w: 2.5482e-05 # gyroscope bias random work noise standard deviation. #4.0e-5

g_norm: 9.805 # gravity magnitude

#unsynchronization parameters

estimate_td: 1 # online estimate time offset between camera and imu

td: 0.004 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#loop closure parameters

load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path'

pose_graph_save_path: "/home/zhang/Downloads/output/pose_graph/" # save and load path

save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0

然后参考官方的运行步骤[4]，效果如下(红色为回环轨迹):

VINS-Fusion.png

下一步工作

研究T265的SDK，然后自己写个里程计?