rs_D455相機内外參标定+imu聯合标定

IMU标定

<launch>

<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">

<!--TOPIC名稱和上面一緻-->

<param name="imu_topic" type="string" value= "/camera/imu"/>

<!--imu_name 無所謂-->

<param name="imu_name" type="string" value= "d435i"/>

<!--标定結果存放路徑-->

<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>

<!--資料錄制時間-min-->

<param name="max_time_min" type="int" value= "120"/>

<!--采樣頻率，即是IMU頻率，采樣頻率可以使用rostopic hz /camera/imu檢視，設定為400，為後面的rosbag play播放頻率-->

<param name="max_cluster" type="int" value= "400"/>

</node>

</launch>
           

原文連結：https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064

參考師姐文章rs相機＋imu内外參标定（1-11）

單目标定

标定步驟：

要盡量保持靜止，移動要緩慢

上下三次，左右三次，翻轉三次，上下平移三次，左右平移三次，前後三次，随機移動多次（任意）

1.kalibr工具生成标定闆,（需要生成棋盤格）：

kalibr_create_target_pdf --type 'apriltag' --nx 6 --ny 6 --tsize 0.08 --tspace 0.3

2.下載下傳&列印标定闆

https://github.com/ethz-asl/kalibr/wiki/downloads

https://github.com/ethz-asl/kalibr/wiki/calibration-targets（三個參數chekboard, circlegrid, aprilgrid說明，使用的是aprilgrid）

target_type: 'aprilgrid' #gridtype

tagCols: 6 #number of apriltags

tagRows: 6 #number of apriltags

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

tagSpacing: 0.00286 #ratio of space between tags to tagSize

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

3.确定适合距離

roslaunch realsense2_camera rs_camera.launch

rviz
           

左側Fixed Frame選擇camera_link

左下角 add --> By topic --> /camera/color/image_raw/ --> 輕按兩下Camera ，找一個适合的能拍到棋盤格的距離

關閉

4.修改相機幀數

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

修改相機幀數到4hz，這裡采用了新的話題去釋出：/color 第5步錄制寫到/color話題。

5.錄制ROS資料包

rosbag record -O camd455i /color
           

錄一分鐘就好了，錄的時候對着标定闆移動，按下ctrl+c 結束錄制，看到目前檔案夾會出現camd455i.bag資料包。

6.使用Kalibr進行标定（前面的都在tx2上進行，這一步在筆記本電腦上進行）

在剛剛建立的ROS中的Kalibr的工作空間，執行source devel/setup.sh，執行：

kalibr_calibrate_cameras --target aprilgrid.yaml --bag camd455i.bag --models pinhole-radtan --topics /color --show-extraction
           

pinhole-radtan指的是相機模型和畸變模型

标定完之後可以發現目前目錄下出現三個檔案，就是标定的結果：

camchain-…camd435i.yaml

results-cam-…camd435i.txt

report-cam-…camd435i.pdf

拍攝的時候，角度不要變化太大，不然會初始化失敗并且優化發散。（[ERROR] Did not converge in maxIterations... restarting...）。因為初始化的時候就必須要得到一個較好的估計，但如果有太大角度的圖檔，很容易得到錯誤的初值，是以後面就會優化失敗。

标定參數為：

cam0:

cam_overlaps: []

camera_model: pinhole

distortion_coeffs: [-0.13104112755856126, 0.020606715483751877, -0.002771690056311741,

0.001163213320750032]

distortion_model: radtan

intrinsics: [260.9278068132713, 263.94361004788766, 301.7212486187008, 199.01710004608793]

resolution: [640, 480]

rostopic: /color
           

IMU+單目相機标定

官方教程：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration

标定完camera，IMU，得到相應的yaml檔案（即分别為相機和IMU的參數）

标定步驟：

上下三次，左右三次，翻轉三次，上下平移三次，左右平移三次，前後三次，随機移動多次（任意）

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.錄制資料包

先把目錄移動到剛剛建立的kalibr工作環境目錄下。

然後執行source devel/setup.sh

rosbag record -b 4096 -O dynamic /color /imu
           

3.重寫yaml檔案

标定需要三個檔案，一個是相機的标定檔案，一個是IMU的标定檔案，一個是錄制的資料包

同樣是用kalibr标定的，camchain-camd435i.yaml，是以直接使用相機yaml檔案即可：

（1）相機标定的yaml檔案

cam0:

cam_overlaps: []

camera_model: pinhole

distortion_coeffs: [-0.14627678771168612, 0.031132819617662677, -0.0016199154527738965,

-0.01257776985511912]

distortion_model: radtan

intrinsics: [259.05353479443266, 256.15264741602005, 290.7955146414971, 234.21114661849504]

resolution: [640, 480]

rostopic: /color
           

（2）建立imu.yaml

在kalibr工作環境目錄下建立imu.yaml檔案：

#Accelerometers

accelerometer_noise_density: 2.0477290485501922e-02 #Noise density (continuous-time)

accelerometer_random_walk: 4.2308969579290693e-04 #Bias random walk

#Gyroscopes

gyroscope_noise_density: 2.2488785808195085e-03 #Noise density (continuous-time)

gyroscope_random_walk: 1.5385085422768701e-05 #Bias random walk

rostopic: /imu #the IMU ROS topic

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

其中以下IMU标定參數已知，将結果對應地填到以上imu.yaml中去：

Gyr:

avg-axis:

gyr_n: 2.2488785808195085e-03

gyr_w: 1.5385085422768701e-05

Acc:

avg-axis:

acc_n: 2.0477290485501922e-02

acc_w: 4.2308969579290693e-04
           

4.使用Kalibr進行标定

在Kalibr工作目錄下執行 source devel/setup.sh

執行指令：

kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camd455i.yaml --imu imu.yaml --bag dynamic.bag --show-extraction

kalibr_calibrate_imu_camera \

--target aprilgrid.yaml \

--cam camchain-camd455i.yaml \

--imu imu.yaml \

--bag imu_cam.bag \

--show-extraction
           

可以錄制2分鐘，生成檔案名字為：

camchain-imucam-dynamic.yaml

其中T_cam_imu，就是我們需要的外參。

錯誤：

ImportError: No module named scipy.optimize

解決辦法：

sudo apt-get install python-scipy
           

做完這些就可以耐心等待結果了。。。

Transformation (cam0):

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

T_ci: (imu0 to cam0):

[[ 0.99579733 0.0648917 0.06462771 -0.01794528]

[-0.06212431 0.99710033 -0.04394886 0.01608132]

[-0.06729222 0.03974921 0.9969412 -0.07784444]

[ 0. 0. 0. 1. ]]

T_ic: (cam0 to imu0):

[[ 0.99579733 -0.06212431 -0.06729222 0.01363058]

[ 0.0648917 0.99710033 0.03974921 -0.01177593]

[ 0.06462771 -0.04394886 0.9969412 0.07947285]

[ 0. 0. 0. 1. ]]
           

雙目标定

步驟一：下載下傳列印标定闆或購買标定闆

到https://github.com/ethz-asl/kalibr/wiki/downloads選擇

下載下傳，然後縮放到40%，用A4紙就可以列印出來

原始pdf的格子參數是：

6*6的格子

大格子邊長：5.5cm

小格子邊長：1.65cm

小格子與大格子邊長比例：0.3

調整後的格子參數是：

大格子邊長：2.2cm

小格子邊長：0.66cm

小格子與大格子邊長比例：0.3

但這隻是理想情況，實際情況還得實際測量。

建立aprilgrid.yaml檔案，格式參考上圖的yaml，内容展示如下： 

target_type: 'aprilgrid' #gridtype

tagCols: 6 #number of apriltags

tagRows: 6 #number of apriltags

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

tagSpacing: 0.00244 #ratio of space between tags to tagSize

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

千萬要自己測量大格子邊長，即tagSize，我測出來實際上是0.024，是以我的檔案裡面tagSize是0.024，請自行更改。

步驟二：啟動關閉結構光

預設開始結構光時，雙目圖像會有很多點，這些點可能對标定有影響，是以使用時需要關閉結構光。

先啟動

roslaunch realsense2_camera rs_camera.launch
           

新打開終端，運作

rosrun rqt_reconfigure rqt_reconfigure
           

打開後将camera->stereo_module中的emitter_enabled設定為off(0) ，展示如下：

步驟三：确定realsense合适放置位置

新打開終端，運作rviz

rviz
           

之後在裡面add rgb和雙目對應的topic，

/camera/color/image_raw、

/camera/infra1/image_rect_raw

/camera/infra2/image_rect_raw
           

之後對準标定闆，嘗試移動realsense，同時要確定标定闆一直在三個圖像當中。錄制過程參考https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop

步驟四：修改相機幀數（官方推薦是4Hz，盡管實際頻率不完全準确，但是不影響結果）

kalibr在處理标定資料的時候要求頻率不能太高，一般為4Hz，我們可以使用如下指令來更改topic的頻率，實際上是将原來的topic以新的頻率轉成新的topic，實際測試infra1才是對應左目相機

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

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left

rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right

注意：這種方式可能導緻不同攝像頭的時間不同步，如果出現這個問題，可以嘗試不做這個操作，不這樣做意味着需要更多的處理時間，這樣的話後面也要相應的更改。

步驟五：錄制ROS資料包

rosbag record -O multicameras_calibration /infra_left /infra_right /color

後面三個topic就是轉換頻率後的topic

步驟六：使用Kalibr标定

先激活環境變量

source devel/setup.bash

然後

kalibr_calibrate_cameras \

--target aprilgrid.yaml \

--bag multicameras_calibration.bag \

--models pinhole-equi pinhole-equi pinhole-equi \

--topics /infra_left /infra_right /color \

--bag-from-to 10 80 \

--show-extraction \

其中–target aprilgrid.yaml是标定闆的配置檔案，注意如果選擇棋格盤，注意targetCols和targetRows表示的是内側角點的數量，不是格子數量。–bag multicameras_calibration.bag是錄制的資料包，models pinhole-equi pinhole-equi pinhole-equi表示三個攝像頭的相機模型和畸變模型（解釋參考https://github.com/ethz-asl/kalibr/wiki/supported-models，根據需要選取）， --topics /infra_left /infra_right /color表示三個攝像頭對應的拍攝的資料話題，–bag-from-to 10 100表示處理bag中10-100秒的資料。–show-extraction表示顯示檢測特征點的過程，這些參數可以相應的調整。

可能出現以下報錯：

1、報錯：Camera are not connected through mutual observations, please check the dataset, Maybe adjust the approx. sync. tolerance.

解決辦法：原因應該是各個攝像頭資料不同步，解決辦法為在标定指令中添加–approx-sync 0.04，更改後的标定指令為：

kalibr_calibrate_cameras \

--target aprilgrid.yaml \

--bag multicameras_calibration.bag \

--models pinhole-equi pinhole-equi pinhole-equi \

--topics /infra_left /infra_right /color \

--bag-from-to 10 80 \

--show-extraction \

--approx-sync 0.04

其中0.04可以看情況調整到0.1，如果還是不行可能是錄制的資料有問題，需要重新錄制，確定标定闆一直在圖像當中，并且運動不要太劇烈。如果還是不行嘗試不要調整topic頻率

參考https://blog.csdn.net/HUST_lc/article/details/96144499和https://blog.csdn.net/HUST_lc/article/details/96144499評論

3、使用棋格标定闆标定時出現如下錯誤

Did not converge in maxIterations... restarting...

等十幾分鐘。。。。。。

最終得到的結果為三個檔案：

雙目+IMU标定（跟單目差不多）

步驟一：編寫camchain.yaml

格式參考Kalibr官方教程https://github.com/ethz-asl/kalibr/wiki/yaml-formats中的chain.yaml，具體的參數參考上面得到的yaml檔案，沒有的參數可以删除，最終結果示例如下：

cam0:

camera_model: pinhole

intrinsics: [425.9022457154398, 425.51203910141476, 320.7722078245714, 234.52032563515024]

distortion_model: equidistant

distortion_coeffs: [0.28012961504219924, 0.776522889083524, -3.555039585283302, 7.122425751506347]

rostopic: /infra_left

resolution: [640, 480]

cam1:

camera_model: pinhole

intrinsics: [430.5354779160687, 429.81266841318336, 318.5487581769465, 232.5397023354142]

distortion_model: equidistant

distortion_coeffs: [0.2913961623966211, 0.7172454259365787, -3.7209659434658295, 7.615803448415106]

T_cn_cnm1:

[ 0.9999886252230528, 7.228145915638569e-05, -0.004769087951971224,0.054624373296219914]

[-6.78405772070143e-05, 0.9999995640055901, 0.0009313357504042449, 0.002552511727883378]

[0.004769153190982554,-0.0009310016189884111, 0.9999881941372211,0.004273862007202206]

[0.0, 0.0, 0.0, 1.0]

rostopic: /infra_right

resolution: [640, 480]
           

步驟二：編寫imu.yaml，

格式參考https://github.com/ethz-asl/kalibr/wiki/yaml-formats中的imu.yaml，具體參數使用之前imu标定得到的參數，示例如下：

#Accelerometers

accelerometer_noise_density: 2.8250053766610776e-02 #Noise density (continuous-time)

accelerometer_random_walk: 7.8925155899657628e-04 #Bias random walk

#Gyroscopes

gyroscope_noise_density: 2.3539521240749008e-03 #Noise density (continuous-time)

gyroscope_random_walk: 2.2003805724014335e-05 #Bias random walk

rostopic: /imu #the IMU ROS topic

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

步驟三：準備好之前的aprilgrid.yaml，示例如下：

target_type: 'aprilgrid' #gridtype

tagCols: 6 #number of apriltags

tagRows: 6 #number of apriltags

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

tagSpacing: 0.00299 #ratio of space between tags to tagSize

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

步驟四：複制realsense-ros包中rs_camera.launch，重命名為rs_imu_stereo.launch，更改内容為

将

<arg name="enable_sync" default="false"/>

改為：

<arg name="enable_sync" default="true"/>

這樣來使imu和雙目資料時間對齊

将

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

改為

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

這樣來保證會有imu話題

步驟五：啟動realsense

roslaunch realsense2_camera rs_imu_stereo.launch

步驟六：關閉IR結構光，參考上面

rosrun rqt_reconfigure rqt_reconfigure 
           

打開後将camera->stereo_module中的emitter_enabled設定為off(0) ，展示如下：

步驟七：打開rviz，add imu topic和infra1 topic以及infra2 topic，同時調整realsense位置，

要確定雙目圖像資料一直包含标定闆全部内容

步驟八：調整imu和雙目topic的釋出頻率以及以新的topic名釋出它們

其中雙目圖像的釋出頻率改為20Hz，imu釋出頻率改為200Hz

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left

rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right

rosrun topic_tools throttle messages /camera/imu 200.0 /imu
           

注意：，這種調整頻率的方式隻是理想結果，通過rostopic hz topic名可以檢視實際的頻率，可以發現實際頻率和設定的頻率并不一定相同，但可以先這樣，知道如何調整的還望告知。

步驟九: 開始錄制資料包，

錄制過程參考[https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop]，同樣注意雙目圖像在整個過程要包含整個标定闆，同時運動不能太快，這樣會造成圖像過于模糊，在前後左右上下方向來回移動，錄制大概90秒，前後15秒等下可以不使用。錄制指令為：

rosbag record -O imu_stereo.bag /infra_left /infra_right /imu

步驟十：開始進行标定

标定指令為

kalibr_calibrate_imu_camera --bag [filename.bag] --cam [camchain.yaml] --imu [imu.yaml] --target [target.yaml] --bag-from-to 15 75 --show-extraction

kalibr_calibrate_imu_camera --bag imu_stereo.bag --cam camchain.yaml --imu imu.yaml --target aprilgrid.yaml --bag-from-to 15 75 --show-extraction

相應參數需要相應更改，target.yaml對應april_6x6_A4.yaml檔案

最終得到的結果為是得打yaml，txt，和pdf檔案

飛控IMU+D455标定

D455相機IMU資料：

T_ic: (cam0 to imu0):

[[-0.07683649 -0.05913403 0.99528856 -0.00011369]

[-0.98555081 0.15563515 -0.06683784 -0.00001289]

[-0.1509495 -0.98604303 -0.07023805 0.00004632]

[ 0. 0. 0. 1. ]]
           

使用realsense-viewer錄制并标定rgb相機

步驟一：打開realsense并設定好bag包record儲存路徑，設定如下：設定好rbg相機的分辨率、采樣頻率後按record記錄資料，錄制約90秒

步驟二：使用Kalibr标定

先激活環境變量

source devel/setup.bash

然後

kalibr_calibrate_cameras --target ~/project/calibration/rgb/april_6x6_A4.yaml --bag ~/disk/datasets/realsense-dataset/calibration/rgb/20200526_131810.bag --models pinhole-equi --topics /device_0/sensor_1/Color_0/image/data --bag-from-to 20 80 --show-extraction

其中–target aprilgrid.yaml是标定闆的配置檔案，注意如果選擇棋格盤，注意targetCols和targetRows表示的是内側角點的數量，不是格子數量。–bag imu_stereo.bag是錄制的資料包，models pinhole-equi pinhole-equi pinhole-equi表示三個攝像頭的相機模型和畸變模型（解釋參考https://github.com/ethz-asl/kalibr/wiki/supported-models，根據需要選取）， --topics /device_0/sensor_1/Color_0/image/data表示拍攝的rgb相機資料話題，–bag-from-to 20 80表示處理bag中20-80秒的資料。–show-extraction表示顯示檢測特征點的過程，這些參數可以相應的調整。

注意：

pinhole-radtan: 最常見的針孔模型+布朗畸變模型, 适用于大多數的角度小于120的相機, 其中畸變參數包含了徑向畸變k1,k2和切向畸變p1,p2; 如果相機的畸變情況不是很嚴重,這個模型基本都可以; 比如我的DOV為150的相機, 也可以用這個且去畸變效果很好;

pinhole-equi:針孔模型＋等距畸變模型，也就是ＫＢ模型所需要選擇的類型，該模型的使用範圍也很廣，大部分的魚眼鏡頭也可以，注意８參數的ＫＢ模型的畸變參數為k1,k2,k3,k4，雖然也是四個數，但與前一個模型不同的是，這裡隻有徑向畸變的參數，而沒有切向畸變tangential distortion，投影時對應的公式也不同；同時這也是opencv中cv::fisheye使用的模型；