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查看，设置为200，为后面的rosbag play播放频率-->

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

</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使用的模型；