一、IMU 標(biāo)定
使用 imu_utils 功能包標(biāo)定 IMU，由于imu_utils功能包的編譯依賴于code_utils，需要先編譯code_utils，主要參考
相機(jī)與IMU聯(lián)合標(biāo)定_熊貓飛天的博客-CSDN博客
Ubuntu20.04編譯并運(yùn)行imu_utils，并且標(biāo)定IMU_學(xué)無(wú)止境的小龜?shù)牟┛?CSDN博客
1.1 編譯 code_utils
創(chuàng)建工作空間
mkdir -p ~/catkin_ws/src/imu_calib/src/
cd ~/catkin_ws/src/imu_calib/src
git clone https://github.com/gaowenliang/code_utils.git1.1.1 修改 CMakeLists.txt 文件
修改 set(CMAKE_CXX_FLAGS "-std=c++11") 為 set(CMAKE_CXX_FLAGS "-std=c++14")
修改 #include "backward.hpp" 為 include “code_utils/backward.hpp”
?如果安裝的是 OpenCV 4.x.x 則需要修改一些全局變量的名稱，終端輸入
cd ~/catkin_ws/src/imu_calib/src/code_utils/
sed -i 's/CV_LOAD_IMAGE_UNCHANGED/cv::IMREAD_UNCHANGED/g' `grep CV_LOAD_IMAGE_UNCHANGED -rl ./`
sed -i 's/CV_LOAD_IMAGE_GRAYSCALE/cv::IMREAD_GRAYSCALE/g' `grep CV_LOAD_IMAGE_GRAYSCALE -rl ./`
sed -i 's/CV_MINMAX/cv::NORM_MINMAX/g' `grep CV_MINMAX -rl ./`安裝依賴
sudo apt-get install libdw-dev編譯 code_utils
mkdir -p ~/catkin_ws/src/imu_calib/
catkin_make1.2 編譯 imu_utils
mkdir -p ~/catkin_ws/src/imu_calib/src/
cd ~/catkin_ws/src/imu_calib/src
git clone https://github.com/gaowenliang/imu_utils.git修改 CMakeLists.txt 文件
修改 set(CMAKE_CXX_FLAGS "-std=c++11") 為 set(CMAKE_CXX_FLAGS "-std=c++14")
修改 imu_an.cpp 文件
添加頭文件：#include <fstream>
編譯 imu_utils
mkdir -p ~/catkin_ws/src/imu_calib/
catkin_make1.3 錄制 imu 數(shù)據(jù)集
創(chuàng)建錄制的數(shù)據(jù)保存路徑
mkdir ~/catkin_ws/src/imu_calib/bag/
cd imu_calib/bag/啟動(dòng)相應(yīng)的設(shè)備開(kāi)始發(fā)布 imu 數(shù)據(jù)，d435i 相機(jī)可以啟用 realsense-ros 發(fā)布相機(jī) imu 數(shù)據(jù)
roslaunch realsense2_camera rs_camera.launch靜止情況下采集IMU的數(shù)據(jù)，并錄制為ROS包，采集的時(shí)間 2小時(shí) 左右
rosbag record /camera/imu -O ~/catkin_ws/src/imu_calib/bag/imu.bag在 ~/imu_calib/src/imu_utils/launch 路徑下創(chuàng)建如下 d435i.launch 文件
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<!--訂閱的imu話題-->
<param name="imu_topic" type="string" value= "/camera/imu"/>
<!--標(biāo)定結(jié)果的名稱-->
<param name="imu_name" type="string" value= "d435i"/>
<!--標(biāo)定結(jié)果存放路徑-->
<param name="data_save_path" type="string" value= "$(find imu_utils)/../../bag/d435i/"/>
<!--數(shù)據(jù)錄制時(shí)間-min 120分鐘 可以自行修改 一般要大于60-->
<param name="max_time_min" type="int" value= "120"/>
<!--imu采樣頻率，設(shè)置為400-->
<param name="max_cluster" type="int" value= "400"/>
</node>
</launch>在 imu 數(shù)據(jù)采集完畢后（錄制時(shí)間兩小時(shí)左右），啟動(dòng)上述 launch 文件標(biāo)定 imu 內(nèi)參
roslaunch imu_utils d435i.launch
rosbag play -r 200 ~/catkin_ws/src/imu_calib/bag/imu.bag數(shù)據(jù)包播放結(jié)束之后，在 ~/catkin_ws/src/imu_calib/bag/ 這個(gè)文件夾下會(huì)出現(xiàn)一系列的參數(shù)文件，
打開(kāi) d435i_imu_param.yaml 這個(gè)文件，會(huì)看到計(jì)算出來(lái)的噪聲和隨機(jī)游走的系數(shù)值
至此，IMU的內(nèi)參標(biāo)定和記錄結(jié)束。
二、相機(jī)標(biāo)定
2.1 編譯 kalibr
使用 kalibr 功能包標(biāo)定相機(jī)，編譯 kalibr，主要參考
https://github.com/ethz-asl/kalibr/wiki/installation
創(chuàng)建工作空間并下載源碼
mkdir -p ~/catkin_ws/src/kalibr/src/ && cd ~/catkin_ws/src/kalibr/src/
git clone https://github.com/ethz-asl/kalibr.git編譯 kalibr
cd ~/catkin_ws/src/kalibr/ && catkin build -DCMAKE_BUILD_TYPE=Release -j42.2 制作標(biāo)定板
終端輸入
source ~/catkin_ws/src/kalibr/devel/setup.bash
cd ~/catkin_ws/src/kalibr/bag/stereo/
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3不論是打印PDF標(biāo)定還是直接在電腦里面打開(kāi)PDF標(biāo)定，都要實(shí)際測(cè)量一下二維碼方格和小方格的的長(zhǎng)度，再填到y(tǒng)aml文件里面，
--type apriltag??????????????? 標(biāo)定板類型
--nx [NUM_COLS]??????????????? 列個(gè)數(shù)
--ny [NUM_ROWS]??????????????? 行個(gè)數(shù)
--tsize [TAG_WIDTH_M]????????? 二維碼方格長(zhǎng)度，單位m
--tspace [TAG_SPACING_PERCENT] 小方格與二維碼方格長(zhǎng)度比例
新建 april_6x6_A4.yaml 文件，格式參考上面的yaml，內(nèi)容展示如下：
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.0318 #size of apriltag, edge to edge [m] 要親自拿尺子量一下
tagSpacing: 0.305 #ratio of space between tags to tagSize千萬(wàn)要自己量一下 tagSize?。。?/p>
2.3 錄制數(shù)據(jù)集
啟動(dòng)相應(yīng)的設(shè)備開(kāi)始發(fā)布 相機(jī) 數(shù)據(jù)，d435i 相機(jī)可以啟用 realsense-ros 發(fā)布相機(jī) imu 數(shù)據(jù)
roslaunch realsense2_camera rs_camera.launchkalibr 在處理標(biāo)定數(shù)據(jù)的時(shí)候要求頻率不能太高，官方推薦是4Hz（盡管實(shí)際頻率不完全準(zhǔn)確，但是不影響結(jié)果），我們可以使用如下命令來(lái)更改topic的頻率，實(shí)際上是將原來(lái)的topic以新的頻率轉(zhuǎn)成新的topic， infra1 對(duì)應(yīng)左目相機(jī)。
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創(chuàng)建數(shù)據(jù)保存路徑，并錄制雙目圖像數(shù)據(jù)
mkdir -p ~/kalibr/bag/stereo/
rosbag record /infra_left /infra_right -O ~/catkin_ws/src/kalibr/bag/stereo/stereo.bag錄制操作參考
Kalibr相機(jī)及IMU校準(zhǔn)教程(Tutorial: IMU-camera calibration)_嗶哩嗶哩_bilibili
總結(jié)下來(lái)就是偏航角左右擺動(dòng)2次，俯仰角擺動(dòng)2次，滾轉(zhuǎn)角擺動(dòng)2次，上下移動(dòng)2次，左右移動(dòng)2次，前后移動(dòng)2次，然后自由移動(dòng)一段時(shí)間，擺動(dòng)幅度要大一點(diǎn)，讓視角變化大一點(diǎn)，但是移動(dòng)要緩慢一點(diǎn)，同時(shí)要保證標(biāo)定板在2個(gè)相機(jī)視野內(nèi)部，整個(gè)標(biāo)定時(shí)間要在90s以上更好，但是優(yōu)化時(shí)間會(huì)比較長(zhǎng)。
2.4 標(biāo)定
錄制完成后使用 kalibr 標(biāo)定
rosrun kalibr kalibr_calibrate_cameras \
--target /home/lilabws001/catkin_ws/src/kalibr/bag/d435i/stereo/april_6x6_A4.yaml \
--bag /home/lilabws001/catkin_ws/src/kalibr/bag/d435i/stereo/stereo.bag \
--models pinhole-radtan pinhole-radtan \
--topics /infra_left /infra_right \
--bag-from-to 10 130 --show-extraction --approx-sync 0.1參數(shù)解釋
- --targt 標(biāo)定板的配置文件路徑
- --bag 采集的數(shù)據(jù)包的路徑
- --models 每個(gè)相機(jī)的模型
- --topics 每個(gè)相機(jī)發(fā)布的話題，需要與前面的相機(jī)模型對(duì)應(yīng)
- --bag-from-to 處理bag中指定時(shí)間段的數(shù)據(jù)
- --show-extraction 表示顯示檢測(cè)特征點(diǎn)的過(guò)程
報(bào)錯(cuò)1：
Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.
[ERROR] [1668944382.174500]: initialization of focal length for cam with topic /color failed
解決：
如果提示不能得到初始焦距的時(shí)候，可以設(shè)置：export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1(終端輸入)。然后運(yùn)行程序，當(dāng)程序運(yùn)行失敗的時(shí)候，它會(huì)提示要你手動(dòng)輸入一個(gè)焦距，Initialization of focal length failed. Provide manual initialization: 這時(shí)手動(dòng)輸入比如 400,給比較大的值，也能收斂。
參考：Realsence D455標(biāo)定并運(yùn)行Vins-Fusion_realsense 自動(dòng)標(biāo)定_呼叫江江的博客-CSDN博客
報(bào)錯(cuò)2：
Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
解決：
應(yīng)該是兩個(gè)相機(jī)時(shí)間不同步導(dǎo)致的，需要調(diào)整參數(shù)：
--approx-sync 0.04
報(bào)錯(cuò)3：
File "/home/lilabws001/catkin_ws/src/kalibr/src/kalibr/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraUtils.py", line 123, in getReprojectionErrorStatistics
??? mean = np.mean(rerr_matrix, 0, dtype=np.float)
? File "/home/lilabws001/.local/lib/python3.8/site-packages/numpy/__init__.py", line 305, in __getattr__
??? raise AttributeError(__former_attrs__[attr])
AttributeError: module 'numpy' has no attribute 'float'.
`np.float` was a deprecated alias for the builtin `float`. To avoid this error in existing code, use `float` by itself. Doing this will not modify any behavior and is safe. If you specifically wanted the numpy scalar type, use `np.float64` here.
解決：
修改 kalibr/aslam_offline_calibration/kalibr/python/kalibr_camera_calibrationCameraUtils.py 文件 line 123 和 line 124
mean = np.mean(rerr_matrix, 0, dtype=np.float64)
std = np.std(rerr_matrix, 0, dtype=np.float64)然后重新標(biāo)定，標(biāo)定完成后會(huì)輸出標(biāo)定結(jié)果。
三、相機(jī) 和 IMU 聯(lián)合標(biāo)定
新建文件夾
mkdir -p ~/catkin_ws/src/kalibr/bag/stereo_imu/3.1 建立標(biāo)定所需的配置文件
首先將前面用于標(biāo)定的標(biāo)定板的配置文件 april_6x6_A4.yaml 復(fù)制到當(dāng)前目錄下，文件內(nèi)容
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.0318 #size of apriltag, edge to edge [m] 要親自拿尺子量一下
tagSpacing: 0.305 #ratio of space between tags to tagSize然后利用前面兩節(jié)標(biāo)定出來(lái)的相機(jī)和 imu 數(shù)據(jù)分別創(chuàng)建用于聯(lián)合標(biāo)定的兩個(gè) yaml 文件
第一個(gè)是 imu 標(biāo)定文件，命名為 imu.yaml，放在 ~/kalibr/bag/stereo_imu/ 目錄下
#Accelerometers
accelerometer_noise_density: 2.3726567696372197e-02 #Noise density (continuous-time)
accelerometer_random_walk: 3.4998014052324268e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 2.9170092608699020e-03 #Noise density (continuous-time)
gyroscope_random_walk: 2.0293647966050773e-05 #Bias random walk
rostopic: /imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)第二個(gè)是 相機(jī) 標(biāo)定文件，命名為 stereo.yaml，放在 ~/kalibr/bag/stereo_imu/ 目錄下
cam0:
camera_model: pinhole
distortion_coeffs: [0.008164119133114047, -0.004262736896205682, 0.00018631722833154752, 0.000787900754729365]
distortion_model: radtan
intrinsics: [382.6730910374852, 382.92071041253627, 322.75543963112193, 236.70194625219574]
resolution: [640, 480]
rostopic: /infra_left
cam1:
T_cn_cnm1:
- [0.999998451671115, 2.8757914694169446e-05, 0.0017594966182482613, -0.050366075624740984]
- [-2.9002408639730846e-05, 0.9999999899284603, 0.00013893140083111915, 6.282865148510808e-05]
- [-0.0017594926051500526, -0.00013898221535954127, 0.9999984424336424, -4.991600269348002e-05]
- [0.0, 0.0, 0.0, 1.0]
camera_model: pinhole
distortion_coeffs: [0.008643399298017006, -0.0051253525048807844, -0.00019751500921053345, 0.00044002401613992687]
distortion_model: radtan
intrinsics: [382.64357095584296, 382.86804296348265, 322.37239440429965, 236.64851650860956]
resolution: [640, 480]
rostopic: /infra_right這兩個(gè)文件的具體數(shù)據(jù)需要于前兩節(jié)的標(biāo)定結(jié)果相對(duì)應(yīng)。
3.2 錄制 相機(jī) 和 imu 的聯(lián)合數(shù)據(jù)
調(diào)整 相機(jī) 和 imu 的 topic 的發(fā)布頻率以及以新的topic名發(fā)布它們，其中雙目圖像的發(fā)布頻率改為20Hz，imu發(fā)布頻率改為200Hz
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
rosrun topic_tools throttle messages /camera/imu 200.0 /imu然后錄制數(shù)據(jù)
rosbag record /infra_left /infra_right /imu -O ~/catkin_ws/src/kalibr/bag/stereo_imu/stereo_imu.bag錄制操作與第二節(jié)相同，參考
Kalibr相機(jī)及IMU校準(zhǔn)教程(Tutorial: IMU-camera calibration)_嗶哩嗶哩_bilibili
總結(jié)下來(lái)就是偏航角左右擺動(dòng)2次，俯仰角擺動(dòng)2次，滾轉(zhuǎn)角擺動(dòng)2次，上下移動(dòng)2次，左右移動(dòng)2次，前后移動(dòng)2次，然后自由移動(dòng)一段時(shí)間，擺動(dòng)幅度要大一點(diǎn)，讓視角變化大一點(diǎn)，但是移動(dòng)要緩慢一點(diǎn)，同時(shí)要保證標(biāo)定板在2個(gè)相機(jī)視野內(nèi)部，整個(gè)標(biāo)定時(shí)間要在90s以上更好，但是優(yōu)化時(shí)間會(huì)比較長(zhǎng)。
3.3 聯(lián)合標(biāo)定 相機(jī) 和 imu
錄制完成后，終端輸入
rosrun kalibr kalibr_calibrate_imu_camera \
--target /home/lilabws001/catkin_ws/src/kalibr/bag/stereo_imu/april_6x6_A4.yaml \
--bag /home/lilabws001/catkin_ws/src/kalibr/bag/stereo_imu/stereo_imu.bag \
--cam /home/lilabws001/catkin_ws/src/kalibr/bag/stereo_imu/stereo.yaml \
--imu /home/lilabws001/catkin_ws/src/kalibr/bag/stereo_imu/imu.yaml \
--bag-from-to 10 50 --show-extraction參數(shù)解釋
- --targt 標(biāo)定板的配置文件路徑
- --bag 采集的數(shù)據(jù)包的路徑
- --cam 標(biāo)定好的相機(jī)的參數(shù)文件
- --imu 標(biāo)定好的 imu 的參數(shù)文件
- --bag-from-to 處理bag中指定時(shí)間段的數(shù)據(jù)（時(shí)間太長(zhǎng)要等很久而且結(jié)果可能退化）
- --show-extraction 表示顯示檢測(cè)特征點(diǎn)的過(guò)程
報(bào)錯(cuò)：
File "/usr/lib/python3/dist-packages/scipy/sparse/sputils.py", line 16, in <module>
??? supported_dtypes = [np.typeDict[x] for x in supported_dtypes]
? File "/usr/lib/python3/dist-packages/scipy/sparse/sputils.py", line 16, in <listcomp>
??? supported_dtypes = [np.typeDict[x] for x in supported_dtypes]
? File "/home/lilabws001/.local/lib/python3.8/site-packages/numpy/__init__.py", line 320, in __getattr__
??? raise AttributeError("module {!r} has no attribute "
AttributeError: module 'numpy' has no attribute 'typeDict'
解決：
numpy 版本過(guò)高，安裝較低版本的 numpy
pip3 install numpy==1.21重新標(biāo)定即可。
如果選的時(shí)間太長(zhǎng)要等很久，因?yàn)榻Y(jié)果可能退化
多等一會(huì)即可，輸出標(biāo)定結(jié)果。
文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-704200.html
其中 stereo_imu-results-imucam.txt 內(nèi)容為標(biāo)定結(jié)果文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-704200.html
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.1104504565760671, median 0.10931046996879386, std: 0.04566466456955288
Reprojection error (cam1): mean 0.10568403044796316, median 0.10371974631938084, std: 0.04481417386193855
Gyroscope error (imu0): mean 0.0013850311184222608, median 2.5661565262693863e-06, std: 0.009802423645836557
Accelerometer error (imu0): mean 0.001268643166366196, median 4.695420807691451e-07, std: 0.00974036762203694
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.1104504565760671, median 0.10931046996879386, std: 0.04566466456955288
Reprojection error (cam1) [px]: mean 0.10568403044796316, median 0.10371974631938084, std: 0.04481417386193855
Gyroscope error (imu0) [rad/s]: mean 5.713632942751922e-05, median 1.058609894733102e-07, std: 0.00040437683974539325
Accelerometer error (imu0) [m/s^2]: mean 0.0004256860317308491, median 1.5755218677114464e-07, std: 0.0032683252080259934
Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[ 0.99991885 -0.00448156 -0.01192529 -0.00263335]
[ 0.00454447 0.99997587 0.00525384 -0.00174852]
[ 0.01190145 -0.00530761 0.99991509 -0.00021396]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[ 0.99991885 0.00454447 0.01190145 0.00264362]
[-0.00448156 0.99997587 -0.00530761 0.00173554]
[-0.01192529 0.00525384 0.99991509 0.00019172]
[ 0. 0. 0. 1. ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.002278866295546706
Transformation (cam1):
-----------------------
T_ci: (imu0 to cam1):
[[ 0.99992565 -0.0044584 -0.01134993 -0.05300071]
[ 0.00452389 0.99997323 0.00575127 -0.00163601]
[ 0.01132399 -0.00580219 0.99991905 -0.00023653]
[ 0. 0. 0. 1. ]]
T_ic: (cam1 to imu0):
[[ 0.99992565 0.00452389 0.01132399 0.05300685]
[-0.0044584 0.99997323 -0.00580219 0.0013983 ]
[-0.01134993 0.00575127 0.99991905 -0.00035563]
[ 0. 0. 0. 1. ]]
timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
0.00246986588204672
Baselines:
----------
Baseline (cam0 to cam1):
[[ 0.99999983 0.00002622 0.00057526 -0.05036719]
[-0.0000265 0.99999988 0.00049716 0.00011254]
[-0.00057525 -0.00049718 0.99999971 -0.00002496]
[ 0. 0. 0. 1. ]]
baseline norm: 0.05036732476881377 [m]
Gravity vector in target coords: [m/s^2]
[-0.0908983 -9.80442883 0.18258076]
Calibration configuration
=========================
cam0
-----
Camera model: pinhole
Focal length: [382.17500647201865, 382.4214301817554]
Principal point: [322.86349593743256, 236.54094094752824]
Distortion model: radtan
Distortion coefficients: [0.005773123668491621, -0.0040545501820581885, 0.00028207298182264084, 0.0008053010502294262]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.0318 [m]
Spacing 0.009699000000000001 [m]
cam1
-----
Camera model: pinhole
Focal length: [382.2362024108845, 382.43170351451005]
Principal point: [322.9638181263497, 236.36811655369087]
Distortion model: radtan
Distortion coefficients: [0.006243739765081835, -0.004482994321431694, -0.0003470496074590888, 0.0006688633081104086]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.0318 [m]
Spacing 0.009699000000000001 [m]
IMU configuration
=================
IMU0:
----------------------------
Model: calibrated
Update rate: 200.0
Accelerometer:
Noise density: 0.023726567696372197
Noise density (discrete): 0.3355443382477292
Random walk: 0.0003499801405232427
Gyroscope:
Noise density: 0.002917009260869902
Noise density (discrete): 0.04125274058290133
Random walk: 2.0293647966050773e-05
T_ib (imu0 to imu0)
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]]
time offset with respect to IMU0: 0.0 [s]
到了這里，關(guān)于d435i 相機(jī)和imu標(biāo)定的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
