d435i 相机和imu标定

想要个小姑娘2023-09-10 12:32

一、IMU 标定

使用 imu_utils 功能包标定 IMU，由于imu_utils功能包的编译依赖于code_utils，需要先编译code_utils，主要参考

相机与IMU联合标定_熊猫飞天的博客-CSDN博客

Ubuntu20.04编译并运行imu_utils，并且标定IMU_学无止境的小龟的博客-CSDN博客

1.1 编译 code_utils

创建工作空间

Go 复制代码 
mkdir -p ~/catkin_ws/src/imu_calib/src/
cd ~/catkin_ws/src/imu_calib/src
git clone https://github.com/gaowenliang/code_utils.git

1.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 则需要修改一些全局变量的名称，终端输入

Go 复制代码 
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

Go 复制代码 
mkdir -p ~/catkin_ws/src/imu_calib/
catkin_make

1.2 编译 imu_utils

Go 复制代码 
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

Go 复制代码 
mkdir -p ~/catkin_ws/src/imu_calib/
catkin_make

1.3 录制 imu 数据集

创建录制的数据保存路径

Go 复制代码 
mkdir ~/catkin_ws/src/imu_calib/bag/
cd imu_calib/bag/

启动相应的设备开始发布 imu 数据，d435i 相机可以启用 realsense-ros 发布相机 imu 数据

roslaunch realsense2_camera rs_camera.launch

静止情况下采集IMU的数据，并录制为ROS包，采集的时间 2小时 左右

Go 复制代码 
rosbag record /camera/imu -O ~/catkin_ws/src/imu_calib/bag/imu.bag

在 ~/imu_calib/src/imu_utils/launch 路径下创建如下 d435i.launch 文件

Go 复制代码 
<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <!--订阅的imu话题-->
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <!--标定结果的名称-->
        <param name="imu_name" type="string" value= "d435i"/>
        <!--标定结果存放路径-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/../../bag/d435i/"/>
        <!--数据录制时间-min 120分钟 可以自行修改 一般要大于60-->
        <param name="max_time_min" type="int" value= "120"/>
        <!--imu采样频率，设置为400-->
        <param name="max_cluster" type="int" value= "400"/>
    </node>
</launch>

在 imu 数据采集完毕后（录制时间两小时左右），启动上述 launch 文件标定 imu 内参

Go 复制代码 
roslaunch imu_utils d435i.launch
rosbag play -r 200 ~/catkin_ws/src/imu_calib/bag/imu.bag

数据包播放结束之后，在 ~/catkin_ws/src/imu_calib/bag/ 这个文件夹下会出现一系列的参数文件，

打开d435i_imu_param.yaml这个文件，会看到计算出来的噪声和随机游走的系数值

至此，IMU的内参标定和记录结束。

二、相机标定

2.1 编译 kalibr

使用 kalibr 功能包标定相机，编译 kalibr，主要参考

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

创建工作空间并下载源码

Go 复制代码 
mkdir -p ~/catkin_ws/src/kalibr/src/ && cd ~/catkin_ws/src/kalibr/src/
git clone https://github.com/ethz-asl/kalibr.git

编译 kalibr

Go 复制代码 
cd ~/catkin_ws/src/kalibr/ && catkin build -DCMAKE_BUILD_TYPE=Release -j4

2.2 制作标定板

终端输入

Go 复制代码 
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标定还是直接在电脑里面打开PDF标定，都要实际测量一下二维码方格和小方格的的长度，再填到yaml文件里面，

--type apriltag 标定板类型

--nx [NUM_COLS] 列个数

--ny [NUM_ROWS] 行个数

--tsize [TAG_WIDTH_M] 二维码方格长度，单位m

--tspace [TAG_SPACING_PERCENT] 小方格与二维码方格长度比例

新建 april_6x6_A4.yaml 文件，格式参考上面的yaml，内容展示如下：

Go 复制代码 
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

千万要自己量一下 tagSize！！！

2.3 录制数据集

启动相应的设备开始发布 相机 数据，d435i 相机可以启用 realsense-ros 发布相机 imu 数据

roslaunch realsense2_camera rs_camera.launch

kalibr 在处理标定数据的时候要求频率不能太高，官方推荐是4Hz（尽管实际频率不完全准确，但是不影响结果），我们可以使用如下命令来更改topic的频率，实际上是将原来的topic以新的频率转成新的topic， infra1 对应左目相机。

Go 复制代码 
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

创建数据保存路径，并录制双目图像数据

Go 复制代码 
mkdir -p ~/kalibr/bag/stereo/
rosbag record /infra_left /infra_right -O ~/kalibr/bag/stereo/stereo.bag

录制操作参考

Kalibr相机及IMU校准教程(Tutorial: IMU-camera calibration)_哔哩哔哩_bilibili

总结下来就是偏航角左右摆动2次，俯仰角摆动2次，滚转角摆动2次，上下移动2次，左右移动2次，前后移动2次，然后自由移动一段时间，摆动幅度要大一点，让视角变化大一点，但是移动要缓慢一点，同时要保证标定板在2个相机视野内部，整个标定时间要在90s以上更好，但是优化时间会比较长。

2.4 标定

录制完成后使用 kalibr 标定

Go 复制代码 
rosrun kalibr kalibr_calibrate_cameras \
--target /home/lilabws001/catkin_ws/src/kalibr/bag/stereo/april_6x6_A4.yaml \
--bag  /home/lilabws001/catkin_ws/src/kalibr/bag/stereo/stereo.bag \
--models pinhole-radtan pinhole-radtan \
--topics /infra_left /infra_right \
--bag-from-to 10 130 --show-extraction

参数解释

  • --targt 标定板的配置文件路径
  • --bag 采集的数据包的路径
  • --models 每个相机的模型
  • --topics 每个相机发布的话题，需要与前面的相机模型对应
  • --bag-from-to 处理bag中指定时间段的数据
  • --show-extraction 表示显示检测特征点的过程

报错：

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

解决：

如果提示不能得到初始焦距的时候，可以设置：export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1(终端输入)。然后运行程序，当程序运行失败的时候，它会提示要你手动输入一个焦距，Initialization of focal length failed. Provide manual initialization: 这时手动输入比如 400,给比较大的值，也能收敛。

参考：Realsence D455标定并运行Vins-Fusion_realsense 自动标定_呼叫江江的博客-CSDN博客

标定完成后会输出标定结果。

三、相机 和 IMU 联合标定

新建文件夹

Go 复制代码 
mkdir -p ~/catkin_ws/src/kalibr/bag/stereo_imu/

3.1 建立标定所需的配置文件

首先将前面用于标定的标定板的配置文件 april_6x6_A4.yaml 复制到当前目录下，文件内容

Go 复制代码 
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

然后利用前面两节标定出来的相机和 imu 数据分别创建用于联合标定的两个 yaml 文件

第一个是 imu 标定文件，命名为 imu.yaml，放在 ~/kalibr/bag/stereo_imu/ 目录下

Go 复制代码 
#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)

第二个是 相机 标定文件，命名为 stereo.yaml，放在 ~/kalibr/bag/stereo_imu/ 目录下

Go 复制代码 
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

这两个文件的具体数据需要于前两节的标定结果相对应。

3.2 录制 相机 和 imu 的联合数据

调整 相机 和 imu 的 topic 的发布频率以及以新的topic名发布它们，其中双目图像的发布频率改为20Hz，imu发布频率改为200Hz

Go 复制代码 
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

然后录制数据

Go 复制代码 
rosbag record /infra_left /infra_right /imu -O ~/kalibr/bag/stereo_imu/stereo_imu.bag

录制操作与第二节相同，参考

Kalibr相机及IMU校准教程(Tutorial: IMU-camera calibration)_哔哩哔哩_bilibili

总结下来就是偏航角左右摆动2次，俯仰角摆动2次，滚转角摆动2次，上下移动2次，左右移动2次，前后移动2次，然后自由移动一段时间，摆动幅度要大一点，让视角变化大一点，但是移动要缓慢一点，同时要保证标定板在2个相机视野内部，整个标定时间要在90s以上更好，但是优化时间会比较长。

3.3 联合标定 相机 和 imu

录制完成后，终端输入

Go 复制代码 
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

参数解释

  • --targt 标定板的配置文件路径
  • --bag 采集的数据包的路径
  • --cam 标定好的相机的参数文件
  • --imu 标定好的 imu 的参数文件
  • --bag-from-to 处理bag中指定时间段的数据（时间太长要等很久而且结果可能退化）
  • --show-extraction 表示显示检测特征点的过程

如果选的时间太长要等很久，因为结果可能退化

多等一会即可，输出标定结果。

其中 stereo_imu-results-imucam.txt 内容为标定结果

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]
相关推荐
lqj_本人2 小时前
鸿蒙next版开发：相机开发-录像(ArkTS)
数码相机·华为·harmonyos
lqj_本人11 小时前
鸿蒙next版开发：相机开发-拍照(ArkTS)
数码相机·华为·harmonyos
lqj_本人1 天前
鸿蒙next版开发：相机开发-适配不同折叠状态的摄像头变更(ArkTS)
数码相机·华为·harmonyos
robot-02 天前
工业相机选取
数码相机
小于小于大橙子2 天前
视觉SLAM数学基础
人工智能·数码相机·自动化·自动驾驶·几何学
键.2 天前
vue-h5:在h5中实现相机拍照加上身份证人相框和国徽框
前端·vue.js·数码相机
小于小于大橙子2 天前
光流法与直接法在SLAM中的应用
人工智能·数码相机·自动化·自动驾驶
金蝶软件小李3 天前
图像处理自动渲染代码
数码相机
光电的一只菜鸡3 天前
相机光学（四十）——2x2 Adjacent Pixel Binning
图像处理·数码相机·计算机视觉
QH_ShareHub3 天前
通过交互式网页学习相机成像原理
数码相机·学习
热门推荐
01玄机平台应急响应—webshell查杀02【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总03组基轨迹建模 GBTM的介绍与实现（Stata 或 R）04新版微信小程序获取用户手机号05全面解析：构建基于深度学习的安全帽检测系统（UI界面+YOLO代码+数据集）06Windows10安装PCL1.14.0及点云配准07【TC3xx芯片】TC3xx芯片电源管理系统PMS详解08Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO09基于YOLOv10深度学习的CT扫描图像肾结石智能检测系统【python源码+Pyqt5界面+数据集+训练代码】深度学习实战、目标检测10【软件工程】UML用例图介绍和实例说明