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]
相关推荐
烟锁池塘柳07 小时前
Camera ISP Pipeline（相机图像信号处理管线）
图像处理·数码相机·信号处理
3DVisionary7 小时前
XTOP3D的DIC技术在极端条件下的应用解决方案
数码相机·3d·航空工业·全场应变测量·航空机匣内部四测头同步测量·反射镜辅助dic观测·四测头方案
视觉人机器视觉2 天前
3D与2D机器视觉机械臂引导的区别
人工智能·数码相机·计算机视觉·3d·视觉检测
LabVIEW开发2 天前
LabVIEW开发中的电机控制与相机像素差
数码相机·labview
pixle03 天前
Three.js 快速入门教程【二】透视投影相机
开发语言·javascript·数码相机
go54631584653 天前
python实现将RGB相机与事件相机的照片信息进行融合以进行目标检测
python·数码相机·目标检测
看星猩的柴狗4 天前
ROS-相机话题-获取图像-颜色目标识别与定位-目标跟随-人脸检测
数码相机
彩云的笔记4 天前
相机快门 deepseek
数码相机
视觉人机器视觉4 天前
机器视觉检测中，2D面阵相机和线扫相机的区别
人工智能·数码相机·计算机视觉·3d·视觉检测
虾球xz4 天前
游戏引擎学习第110天
数码相机·学习·游戏引擎
热门推荐
01DeepSeek各版本说明与优缺点分析02如何在WPS和Word/Excel中直接使用DeepSeek功能03DeepSeek本地部署详细指南04太炸裂了！清华大学deepseek从入门到精通使用手册又出第三版了，《普通人如何抓住DeepSeek红利》（无套路，直接下载）05本地部署DeepSeek教程（Mac版本）06DeepSeek r1本地安装全指南07本地化部署AI知识库：基于Ollama+DeepSeek+AnythingLLM保姆级教程08DeepSeek R1本地化部署 Ollama + Chatbox 打造最强 AI 工具09在Windows下安装Ollama并体验DeepSeek r1大模型10本地部署DeepSeek后的调用与删除全攻略