這裏使用的是realsense D435，使用其灰階雙目的部分（把中間红外点阵投影仪遮住應該可以取得更好的效果），廠商有給相機的標定，但由於Kalibr裏可以直接標定相機和imu的相對位置，是vins需要的參數，比自己用尺子量還準，所以用kalibr標定，所以還是使用kalibr標定vins的參數。
主要步驟如下：
操作環境是ros noetic
1. 錄製rosbag（包含的topic包括），imu的發佈頻率200Hz，相機的部分，會用topic_tools降低頻率
2. 用kalibr標定相機
3. 取得realsense廠商標定的值，如果和剛剛標定的差不多（大部分情況應該是這樣的），直接用廠商的就好了
4. 用imu_utils 標定imu（這一步不一定需要做，我用N100直接用）
5. 取得4個文件後，用kalibr標定 相機和imu的相對位置
6. kalibr標定出來的 T_cam_imu 是imu的位置轉成相機座標的變換矩陣，如果是左眼可以視作 T_cam0_imu
7. vins的config文件的body_T_cam0 是 cam0轉成imu座標的變換矩陣，如果是左眼可以視作 T_imu_cam0，也就是kalibr標定出來的矩陣的逆矩陣。再把其他相機內參（用的是realsense廠商標定的值）寫進去就OK了。
----------------------------------------------------------------------------------------------------------------------------
第一步：
1.啓動realsense，查看/camera/infra1/image_rect_raw，/camera/infra2/image_rect_raw 是否正常發佈
2.啓動imu，頻率200Hz，topic設爲/mavros/imu/data_raw，用rostopic hz /mavros/imu/data_raw查看頻率是否是200Hz
3. 使用以下的命令將image的頻率降低，等會錄製的是降低頻率後的topic
export DecreaseFps_LEFT=/decrease_fps/infra1/image_rect_raw
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 5.0 $DecreaseFps_LEFT
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 5.0 $DecreaseFps_RIGHT這裏如果5.0改成4.0，頻率太低等會標定的時候可能會報錯
#Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
使用rviz查看要錄製的image頻道是否正常發佈
錄製rosbag，注意只要錄製對應的頻道就可以裏，不要錄全部的topic，電腦可能會卡
#!/bin/bash
export LEFT_IMAGE_TOPIC=/camera/infra1/image_rect_raw
export RIGHT_IMAGE_TOPIC=/camera/infra2/image_rect_raw
export DecreaseFps_LEFT=/decrease_fps/infra1/image_rect_raw
export DecreaseFps_RIGHT=/decrease_fps/infra2/image_rect_raw
export IMU_TOPIC=/mavros/imu/data_raw
export TARTGET_PROJECT_DIR=/home/hao/Documents/WKS_Current/camera_imu_calib
export BAG_PATH=$TARTGET_PROJECT_DIR/rosbag/images_imu.bag
rm BAG_PATH
#這裏記錄降低Hz後的image topic
rosbag record $DecreaseFps_RIGHT $DecreaseFps_LEFT $IMU_TOPIC -O $BAG_PATH然後把bag的名字從images_imu.bag改成images_imu_0319_4.bag，避免之後被覆蓋
----------------------------------------------------------------------------------------------------------------------------
第二步：
使用rosrun kalibr kalibr_calibrate_cameras 的命令標定
#!/bin/bash
export NEW_BAG_NAME=images_imu_0319_4.bag
# ------------------------------------------------------------------------------
export TARTGET_PROJECT_DIR=/home/hao/Documents/WKS_Current/camera_imu_calib
source $TARTGET_PROJECT_DIR/kalibr_workspace/devel/setup.bash
export NEW_BAG_PATH=$TARTGET_PROJECT_DIR/rosbag/$NEW_BAG_NAME
export LEFT_IMAGE_TOPIC=/camera/infra1/image_rect_raw
export RIGHT_IMAGE_TOPIC=/camera/infra2/image_rect_raw
export DecreaseFps_LEFT=/decrease_fps/infra1/image_rect_raw
export DecreaseFps_RIGHT=/decrease_fps/infra2/image_rect_raw
export YAML=$TARTGET_PROJECT_DIR/config/april/april_4x6_33x33mm.yaml
# ------------------------------------------------------------------------------
rosrun kalibr kalibr_calibrate_cameras \
--target $YAML \
--bag $NEW_BAG_PATH \
--models pinhole-radtan pinhole-radtan \
--topics $DecreaseFps_LEFT $DecreaseFps_RIGHT取得標定後的文件images_imu_0319_4-camchain.yaml
cam0:
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.002523328249852312, -0.004466313471172815, 0.00033642164779608326, 0.00018286390638593217]
distortion_model: radtan
intrinsics: [380.37233363299345, 380.1905369817509, 318.9785130142544, 245.5928492131154]
resolution: [640, 480]
rostopic: /decrease_fps/infra1/image_rect_raw
cam1:
T_cn_cnm1:
- [0.9999999901664477, 8.91089059750877e-05, -0.00010828989704860977, -0.05220575204275552]
- [-8.901819276146816e-05, 0.9999996454143176, 0.0008374049169301635, -3.378684260237077e-05]
- [0.00010836447888656882, -0.0008373952689245711, 0.9999996435130853, 9.858524895562022e-05]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [0.0015411597395611953, -0.002046041757877386, -1.5684484998354423e-05, 0.0006699072164490144]
distortion_model: radtan
intrinsics: [380.4869229059442, 380.30778002633, 319.44882039078743, 245.19338699710772]
resolution: [640, 480]
rostopic: /decrease_fps/infra2/image_rect_raw
這裏標定的結果一般，網上一般說重投影午餐在0.2以下，但是沒關係，最後我們要用的是廠商標定的值，這裏只要確定和廠商標定的值差不多就行了
----------------------------------------------------------------------------------------------------------------------------
第三步：
獲取廠商的標定值
運行相機後，運行rostopic list查看相機發佈的topic
這裏我們要讀取的是
rostopic echo /camera/infra1/camera_info
rostopic echo /camera/infra2/camera_info
rostopic echo /camera/infra1/camera_info
header:
seq: 69
stamp:
secs: 1710820789
nsecs: 283667326
frame_id: "camera_infra1_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [383.7328186035156, 0.0, 320.62640380859375, 0.0, 383.7328186035156, 244.07247924804688, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [383.7328186035156, 0.0, 320.62640380859375, 0.0, 0.0, 383.7328186035156, 244.07247924804688, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
---
*********************************************************************************************
rostopic echo /camera/infra2/camera_info
---
header:
seq: 48
stamp:
secs: 1710820898
nsecs: 994056225
frame_id: "camera_infra1_optical_frame"
height: 480
width: 640
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [383.7328186035156, 0.0, 320.62640380859375, 0.0, 383.7328186035156, 244.07247924804688, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [383.7328186035156, 0.0, 320.62640380859375, -19.216720581054688, 0.0, 383.7328186035156, 244.07247924804688, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi:
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False
---其中的K就是我們要的內參
K: [383.7328186035156, 0.0, 320.62640380859375, 0.0, 383.7328186035156, 244.07247924804688, 0.0, 0.0, 1.0]
K: [383.7328186035156, 0.0, 320.62640380859375, 0.0, 383.7328186035156, 244.07247924804688, 0.0, 0.0, 1.0]
和剛剛標定的
intrinsics: [380.37233363299345, 380.1905369817509, 318.9785130142544, 245.5928492131154]
intrinsics: [380.4869229059442, 380.30778002633, 319.44882039078743, 245.19338699710772]
....差異不大...是吧?
----------------------------------------------------------------------------------------------------------------------------
第四步：
使用imu_utils標定，時間至少2小時，如果vins運行偏差不大，這一步不一定需要
----------------------------------------------------------------------------------------------------------------------------
第五步：
用kalibr標定 相機和imu的相對位置，這裏需要3個yaml , 和剛剛用過的rosbag
#!/bin/bash
export TARTGET_PROJECT_DIR=/home/hao/Documents/WKS_Current/camera_imu_calib
source $TARTGET_PROJECT_DIR/kalibr_workspace/devel/setup.bash
export APRIL_YAML=$TARTGET_PROJECT_DIR/config//april/april_4x6_33x33mm.yaml
export CAM_YAML=$TARTGET_PROJECT_DIR/config/camera/0319_4/images_imu_0319_4-camchain.yaml
export IMU_YAML=$TARTGET_PROJECT_DIR/config/imu/wheeltec_imu_param.yaml
export BAG_PATH=$TARTGET_PROJECT_DIR/rosbag/images_imu_0319_4.bag
rosrun kalibr kalibr_calibrate_imu_camera \
--target $APRIL_YAML \
--cam $CAM_YAML \
--imu $IMU_YAML \
--bag $BAG_PATH其中 APRIL_YAML 是圖形的，可以到這裏https://calib.io/ 在線生成後貼到牆上
april_4x6_33x33mm.yaml
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 4 #number of apriltags
tagSize: 0.033 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize
images_imu_0319_4-camchain-imucam.yaml
cam0:
T_cam_imu:
- [0.0034284049324164734, 0.9995733722504649, 0.02900550857085149, 0.03935877158851322]
- [0.005143224197809104, -0.029022921129674084, 0.9995655142580464, 0.005894251744585931]
- [0.9999808964597631, -0.0032777335058043477, -0.005240532281598431, -0.0946975587360723]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.002523328249852312, -0.004466313471172815, 0.00033642164779608326, 0.00018286390638593217]
distortion_model: radtan
intrinsics: [380.37233363299345, 380.1905369817509, 318.9785130142544, 245.5928492131154]
resolution: [640, 480]
rostopic: /decrease_fps/infra1/image_rect_raw
timeshift_cam_imu: 0.0032990921924898058
cam1:
T_cam_imu:
- [0.003320575377456475, 0.9995711311657842, 0.029095145971751668, -0.012836200822067947]
- [0.005980306103215982, -0.02911463584384641, 0.9995581893609758, 0.005777658963943309]
- [0.9999766045853646, -0.003145110432898224, -0.006074418679215959, -0.09459961045442837]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9999999901664488, 8.91089059750877e-05, -0.00010828989704860977, -0.05220575204275552]
- [-8.901819276146816e-05, 0.9999996454143187, 0.0008374049169301635, -3.378684260237077e-05]
- [0.00010836447888656882, -0.0008373952689245711, 0.9999996435130865, 9.858524895562022e-05]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [0.0015411597395611953, -0.002046041757877386, -1.5684484998354423e-05, 0.0006699072164490144]
distortion_model: radtan
intrinsics: [380.4869229059442, 380.30778002633, 319.44882039078743, 245.19338699710772]
resolution: [640, 480]
rostopic: /decrease_fps/infra2/image_rect_raw
timeshift_cam_imu: 0.003275118484076564
wheeltec_imu_param.yaml
wheeltec_imu_param.yaml的內容可以從廠商哪裏取得或自己標定
#Accelerometers
accelerometer_noise_density: 1.0909715156015328e-02 #Noise density (continuous-time)
accelerometer_random_walk: 4.0347767978459928e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 8.0226069504443656e-04 #Noise density (continuous-time)
gyroscope_random_walk: 4.4153147263889589e-05 #Bias random walk
rostopic: /mavros/imu/data_raw #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
標定後得到文件images_imu_0319_4-camchain-imucam.yaml
cam0:
T_cam_imu:
- [0.0034284049324164734, 0.9995733722504649, 0.02900550857085149, 0.03935877158851322]
- [0.005143224197809104, -0.029022921129674084, 0.9995655142580464, 0.005894251744585931]
- [0.9999808964597631, -0.0032777335058043477, -0.005240532281598431, -0.0946975587360723]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.002523328249852312, -0.004466313471172815, 0.00033642164779608326, 0.00018286390638593217]
distortion_model: radtan
intrinsics: [380.37233363299345, 380.1905369817509, 318.9785130142544, 245.5928492131154]
resolution: [640, 480]
rostopic: /decrease_fps/infra1/image_rect_raw
timeshift_cam_imu: 0.0032990921924898058
cam1:
T_cam_imu:
- [0.003320575377456475, 0.9995711311657842, 0.029095145971751668, -0.012836200822067947]
- [0.005980306103215982, -0.02911463584384641, 0.9995581893609758, 0.005777658963943309]
- [0.9999766045853646, -0.003145110432898224, -0.006074418679215959, -0.09459961045442837]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9999999901664488, 8.91089059750877e-05, -0.00010828989704860977, -0.05220575204275552]
- [-8.901819276146816e-05, 0.9999996454143187, 0.0008374049169301635, -3.378684260237077e-05]
- [0.00010836447888656882, -0.0008373952689245711, 0.9999996435130865, 9.858524895562022e-05]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [0.0015411597395611953, -0.002046041757877386, -1.5684484998354423e-05, 0.0006699072164490144]
distortion_model: radtan
intrinsics: [380.4869229059442, 380.30778002633, 319.44882039078743, 245.19338699710772]
resolution: [640, 480]
rostopic: /decrease_fps/infra2/image_rect_raw
timeshift_cam_imu: 0.003275118484076564----------------------------------------------------------------------------------------------------------------------------
第六步：
將Kalibr標定出來的相機和imu的矩陣，求出逆矩陣後，寫入vins的參數裏
也就是分別求出以下的逆矩陣，直接用Sophus求即可
cam0:
T_cam_imu:
- [0.0034284049324164734, 0.9995733722504649, 0.02900550857085149, 0.03935877158851322]
- [0.005143224197809104, -0.029022921129674084, 0.9995655142580464, 0.005894251744585931]
- [0.9999808964597631, -0.0032777335058043477, -0.005240532281598431, -0.0946975587360723]
- [0.0, 0.0, 0.0, 1.0]
和
cam1:
T_cam_imu:
- [0.003320575377456475, 0.9995711311657842, 0.029095145971751668, -0.012836200822067947]
- [0.005980306103215982, -0.02911463584384641, 0.9995581893609758, 0.005777658963943309]
- [0.9999766045853646, -0.003145110432898224, -0.006074418679215959, -0.09459961045442837]
- [0.0, 0.0, 0.0, 1.0]
求出的逆矩陣分別是
body_T_cam0:
data: [0.0034284,0.00514322,0.999981,0.0945305,0.999573,-0.0290229,-0.00327773,-0.0394813,0.0290055,0.999566,-0.00524053,-0.00752958,0., 0., 0., 1. ]
body_T_cam1：
data: [ 0.00332058,0.00598031,0.999977,0.0946055,0.999571, -0.0291146, -0.00314511, 0.0127014,0.0290951, 0.999558, -0.00607442, -0.00597627,0., 0., 0., 1. ]
----------------------------------------------------------------------------------------------------------------------------
第七步：
修改vins的config，一般需要三個文件
ego_planner_d435_wheeltecImu.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: "/mavros/imu/data_raw"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
#這個文件需要創建
output_path: "~/Documents/vins_output"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# 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
rows: 4
cols: 4
dt: d
data: [0.0034284,0.00514322,0.999981,0.0945305,0.999573,-0.0290229,-0.00327773,-0.0394813,0.0290055,0.999566,-0.00524053,-0.00752958,0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 0.00332058,0.00598031,0.999977,0.0946055,0.999571, -0.0291146, -0.00314511, 0.0127014,0.0290951, 0.999558, -0.00607442, -0.00597627,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: 1 # 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: 0.1 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 0.01 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 0.001 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 0.0001 # 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.05 # 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/fast/savedfiles/output/pose_graph/" # save and load path
save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
left.yaml
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
# D435 廠商標定
projection_parameters:
fx: 383.7328186035156
fy: 383.7328186035156
cx: 320.62640380859375
cy: 244.07247924804688right.yaml
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
# D435 廠商標定
projection_parameters:
fx: 383.7328186035156
fy: 383.7328186035156
cx: 320.62640380859375
cy: 244.07247924804688然後就可以跑vins了
