Realsense D435i RGB+IMU標定

準備工作：

Ubuntu
ros環境
librealsense
realsense-ros
本次標定使用兩個工具，一個是kalibr工具，可以標定相機內參以及相機-IMU外參，另一個是港科大出品的imu_utils，可以標定IMU內參，我們先標定IMU內參，再標定相機內參，最後聯合標定，得到相機-IMU的外參。

一、IMU標定

1.修改rs_camera.launch部分參數

使得IMU數據同步（針對vins_mono運行IMU時間戳混亂的情況imu message ins disorder）
具體步驟：
找到realsense-ros工作環境文件夾中的rs_camera.launch
位置：在 /realsense的catkin工作環境/src/realsense-ros/realsense2_camera/launch/
把rs_camera.launch中的

  <arg name="unite_imu_method"      default="copy"/>

中的 “copy” 修改成"linear_interpolation"

分別打開三個terminal，電腦插上realsense D435i，試着運行以下：

roscore 
roslaunch realsense2_camera rs_camera.launch
rostopic list

在這個第三個terminal下查看是否有 /camera/imu ，就這個的話說明ok

2.配置港科大IMU標定程序的環境

(1)安裝ceres

去官網下載源碼編譯就好了。
https://github.com/ceres-solver/ceres-solver
http://www.ceres-solver.org/

(2)創建ros工作空間

mkdir -p ~/imu_catkin_ws/src
cd ~/imu_catkin_ws/src
catkin_init_workspace
cd ~/imu_catkin_ws 
catkin_make
source ~/imu_catkin_ws/devel/setup.bash

(3)下載code_utils並編譯

cd ~/imu_catkin_ws/src/
git clone https://github.com/gaowenliang/code_utils.git
cd ~/imu_catkin_ws/src 
catkin_make

(4) 下載imu_utils並編譯

cd ~/imu_catkin_ws/src/
git clone https://github.com/gaowenliang/imu_utils.git
cd ~/imu_catkin_ws/src/
catkin_make

3.寫標定參數文件

cd ~/imu_catkin_ws/src/imu_utils/launch
touch d435i_imu_calibration.launch
gedit d435i_imu_calibration.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= "d435i_imu_calibration"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "10"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

需要注意的是max_time_min代表的是標定時間，這裏的單位是分鐘，意思是填10就是代表10分鐘，至少錄十分鐘。

4.錄製IMU數據包

插上相機

roscore 
roslaunch realsense2_camera rs_camera.launch

讓相機靜止，不要動它

cd ~/imu_catkin_ws      //等下錄製到這個文件夾上
rosbag record -O imu_calibration /camera/imu

至少錄製你剛剛設定的時間以上(max_time_min)
比如上面設定10，這裏就要錄足十分鐘
錄製完之後就按下ctrl+c，結束錄製
你會發現當前目錄有一個名爲 imu_calibration.bag的文件

5.使用imu標定工具進行標定

(1)打開一個terminal

source ~/imu_catkin_ws/devel/setup.sh 
rosbag imu_utils d435i_imu_calibration.launch

(2)新打開一個terminal

source ~/imu_catkin_ws/devel/setup.sh 
cd ~/imu_catkin_ws //數據包在這個文件夾下
rosbag play -r 200 imu_calibration.bag

標定完之後，看(1)的terminal，會有一串東西產生，而且該terminal已經可以輸入命令了（意味着標定進程已經結束），說明已經標定好了，這時候要看結果

cd ~/imu_catkin_ws/src/imu_utils/data

裏面有一個文件：
d435i_imu_calibration_imu_param.yaml
就是你想要的IMU標定結果啦

%YAML:1.0
---
type: IMU
name: d435i_imu_calibration
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 3.1820671461855504e-03
      gyr_w: 3.0693398103399251e-05
   x-axis:
      gyr_n: 4.5370714417877871e-03
      gyr_w: 4.0841410937003261e-05
   y-axis:
      gyr_n: 3.3866604818486266e-03
      gyr_w: 3.8353526280536766e-05
   z-axis:
      gyr_n: 1.6224695149202378e-03
      gyr_w: 1.2885257092657719e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6449533258549235e-02
      acc_w: 7.2111910796954259e-04
   x-axis:
      acc_n: 1.9864483668885411e-02
      acc_w: 5.5777874344305667e-04
   y-axis:
      acc_n: 2.3746240477749841e-02
      acc_w: 5.0079781850747836e-04
   z-axis:
      acc_n: 3.5737875629012449e-02
      acc_w: 1.1047807619580928e-03

我們等下只用到其中四個參數：

Gyr:
   avg-axis:
      gyr_n: 3.1820671461855504e-03
      gyr_w: 3.0693398103399251e-05
Acc:
   avg-axis:
      acc_n: 2.6449533258549235e-02
      acc_w: 7.2111910796954259e-04

分別是陀螺儀和加速度計 隨機遊走和 高斯白噪聲的平均值，是IMU噪聲模型中的兩種噪聲。

二、使用kalibr標定相機

1.安裝Kalibr

建議源碼安裝
https://github.com/ethz-asl/kalibr/wiki/installation

有一點不同的就是他是ubuntu14的環境，如果你ubuntu的版本不同， 你需要把下面關於ROS的包都換成你自己的版本。

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-indigo-vision-opencv ros-indigo-image-transport-plugins ros-indigo-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

比如ros-indigo-image-transport-plugins
我的是Ubuntu 18，裝的是ROS Melodic版本，換成：
ros-melodic-image-transport-plugins
即可

2.下載&打印標定板

https://github.com/ethz-asl/kalibr/wiki/downloads
這裏推薦的是 Aprilgrid 標定板。

https://github.com/ethz-asl/kalibr/wiki/calibration-targets
這裏有三個參數的說明，標定板格子大小之類的。

我下載的是Aprilgrid 6x6 0.5x0.5 m (unscaled)
使用adobe acrobat pro dc 軟件自定義縮放40%打印，A4紙剛好能打印出來。

原版的參數是：
6X6 tags 6乘6個格子
一個大格子size=5.5cm
一個小格子spacing=1.65cm

40%的縮放：
6X6 tags
一個大格子size=2.2cm
一個小格子spacing=0.66cm
記得打印出來用尺子量一下，以免出現差錯。
下載官網提供的yaml格式文件，修改參數，也可以直接用我下面的。

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.022           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize

然後把打印的標定紙貼在牆上

3.確定適合距離

打開rviz觀察合理的距離，要求攝像頭能看到標定棋盤格。

roscore 
roslaunch realsense2_camera rs_camera.launch
rviz

沒有安裝rviz就自行安裝一下

sudo apt-get install rviz

左側 Fixed Frame 選擇camera_link
左下角 add --> By topic --> /camera/color/image_raw/ --> 雙擊Camera ，找一個適合的能拍到棋盤格的距離
關閉

4.修改相機幀數

修改相機幀數到4hz(要求圖像頻率不可過高)

rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color

注意這裏是採用了新的話題去發佈：/color
所以下面錄製要寫/color話題

5.錄製ROS數據包

rosbag record -O camd435i /color

這裏沒說錄多少時間，錄個一分鐘就好了，錄的時候對着標定板移動
按下ctrl+c 結束錄製
看到當前文件夾會出現camd435i.bag數據包

6.使用Kalibr進行標定

在剛剛建立的ROS中的Kalibr的工作空間，執行source devel/setup.sh
執行：
注意：請自行修改文件名與位置

kalibr_calibrate_cameras --target /位置/文件名.yaml --bag /位置/camd435i.bag --bag-from-to 26 100 --models pinhole-radtan --topics /color --show-extraction

26-100 指的是錄製的第26秒到100秒這段時間
pinhole-radtan指的是針孔相機模型和畸變模型

標定完之後就可以發現當前目錄下出現了三個文件，就是標定的結果。
camchain-…camd435i.yaml
results-cam-…camd435i.txt
report-cam-…camd435i.pdf

我們看看生成的yaml文件：

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.11114194921960645, -0.2171600270591326, -0.0037942616908056637,
    -0.004000340562604045]
  distortion_model: radtan
  intrinsics: [579.5589935482029, 578.1762234104125, 326.6772381912598, 243.3955018655041]
  resolution: [640, 480]
  rostopic: /color

intrinsic 是內參
distortion_coeffs 是畸變參數

三、camera_imu聯合標定

標定完camera，IMU，得到相應的yaml文件（即分別爲相機和IMU的參數）。
官方教程：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration

1.調節幀率

相機20Hz，IMU200Hz，官方說這個參數比較好，並分別以/color和/imu爲話題名發佈

rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu

2.錄製數據包

官方似乎沒有說錄多久，但是說了每一個方向都要給激勵，可以參考官網的視頻。YouTube看不了的話可以看泡泡機器人搬運過來的視頻。
鏈接在此
先把目錄移動到剛剛建立的kalibr工作環境目錄下。
然後執行source devel/setup.sh

rosbag record -O dynamic /color /imu

3.重寫yaml文件

標定需要三個文件，一個是相機的標定文件，一個是IMU的標定文件，一個是錄製的數據包

(1)相機標定的yaml文件

由於我們剛剛也是在kalibr工作環境下標定的，所以直接用剛剛標定好的相機yaml文件即可。
即上述所說的
camchain-…camd435i.yaml

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.11114194921960645, -0.2171600270591326, -0.0037942616908056637,
    -0.004000340562604045]
  distortion_model: radtan
  intrinsics: [579.5589935482029, 578.1762234104125, 326.6772381912598, 243.3955018655041]
  resolution: [640, 480]
  rostopic: /color

(2)新建imu.yaml

在kalibr工作環境目錄下新建imu.yaml文件，格式如下：
格式如下：

#Accelerometers
accelerometer_noise_density: 1.86e-03   #Noise density (continuous-time)
accelerometer_random_walk:   4.33e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.87e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.66e-05   #Bias random walk

rostopic:                    /imu0      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

已知

Gyr:
   avg-axis:
      gyr_n: 3.1820671461855504e-03
      gyr_w: 3.0693398103399251e-05
Acc:
   avg-axis:
      acc_n: 2.6449533258549235e-02
      acc_w: 7.2111910796954259e-04

把IMU標定結果對應地填到imu.yaml去，如下：

#Accelerometers
accelerometer_noise_density: 2.64e-02   #Noise density (continuous-time)
accelerometer_random_walk:   7.21e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     3.18e-03   #Noise density (continuous-time)
gyroscope_random_walk:       3.07e-05   #Bias random walk

rostopic:                    /imu   #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

4.使用Kalibr進行標定

別忘了先在kalibr工作目錄下執行 source devel/setup.sh
然後下面的參數仍然需要自己修改

kalibr_calibrate_imu_camera --target 標定板位置/標定板名稱.yaml --cam 相機標定位置/相機標定名稱.yaml --imu imu.yaml --bag dynamic.bag --show-extraction

運行時間會根據你錄製數據包的大小和電腦性能而定，我錄製了將近兩分鐘，電腦是四年前的神舟，所以速度較慢，大概用了有半個小時以上，可以去喝杯咖啡再回來看看。

生成好多個文件，主要關注一個文件：

camchain-imucam-dynamic.yaml

裏面有一個T_cam_imu，就是我們要求的外參。

四、總結

前前後後搞了兩天，真的多虧了各位大佬的博客，才能完成這次的標定，Kalibr的官方教程對新手也太不友好了…然而我也寫的不是很詳細，有什麼問題可以在下面留言。下次我們來標定D435i的剩下的雙目攝像頭，敬請期待。

參考：

https://blog.csdn.net/qq_41839222/article/details/86552367
https://blog.csdn.net/qq_25458977/article/details/102806768
https://blog.csdn.net/fang794735225/article/details/92804030
https://blog.csdn.net/qq_36170626/article/details/102998942?utm_source=app
https://blog.csdn.net/kongdaqing1290/article/details/97524087
https://blog.csdn.net/weixin_40628128/article/details/95945945?utm_source=app
https://blog.csdn.net/qq_36170626/article/details/102998942?utm_source=app

Hecttttttttt

發佈了44 篇原創文章 · 獲贊 43 · 訪問量 7萬+

私信關注

Realsense D435i RGB+IMU標定