使用Kalibr標定vins參數

這裏使用的是realsense D435，使用其灰階雙目的部分（需要把中間红外点阵投影仪IR Projector遮住，標定的重投影誤差可以大幅下降），廠商有給相機的標定，但由於Kalibr裏可以直接標定相機和imu的相對位置，是vins需要的參數，比自己用尺子量還準，所以使用kalibr標定vins的參數。

如果使用realsense D435，由於廠商已經標定裏，主要是用Kalibr獲取imu和相機相對位置。如果使用D435i，好像沒有需要用Kalibr的地方，如果使用未標定的相機，使用Kalibr標定相機和相機對imu的變換.

主要步驟如下：
操作環境是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以下，但是沒關係，最後我們要用的是廠商標定的值，這裏只要確定和廠商標定的值差不多就行了

（後來我把IR Projector用黑色的紙遮住後重新錄製rosbag，重投影誤差立馬降到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]
....差異不大...是吧?

----------------------------------------------------------------------------------------------------------------------------

後來把IR Projector遮住後，重投影誤差降到0.2以下，標定的結果：
  intrinsics: [383.5379746421081, 383.358638430082, 323.96639799140695, 246.31269714810153]
  intrinsics: [383.56355491190146, 383.30929084687796, 323.4369010808036, 246.09007244256364]
和原廠標定的更接近了，所以標定不好不要慌，直接拿原廠的用，標定好的也不要膨脹，還是拿原廠的用。

----------------------------------------------------------------------------------------------------------------------------

第四步：
使用imu_utils標定，時間至少2小時，但如果厂商有给参考值（N100有，pixhawk好像没有）， 不需要这一步

----------------------------------------------------------------------------------------------------------------------------

第五步：
用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 

這一塊實測如果改成自己標定後的，反而更容易飄走，不知道爲何
#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

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.07247924804688

right.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了