D435i标定

一、三部分标定

路线：相机内参标定→imu内参标定→相机，imu外参联合标定→结果评定

前期步骤：Kalibr环节搭建，D435i驱动安装，ORB-SLAM3环境搭建

有的人还会关闭结构光，在realsense_ws里的rs.launch里1关即可，emitter_enabled设置为off(0)

（一）相机标定

运行标定程序还是先需要知道需要输入什么量：

1.bag文件（待标定相机录得的数据包）

2.yawl文件（之前下载的，要复制到与bag一个文件夹，这个文件像一个先验文件，标定结束后还回生成一个yawl文件，算是后验，yawl文件需要自己进行修改）

3.相机模型文件，就是你所需要的内参的模板，针孔相机（手机）就选pinhole-radtan（标定相机命令里面的模型必须选对，本文选取的是--models pinhole-radtan，否则会报错，这一步非常重要。具体模型参考https://github.com/ethz-asl/kalibr/wiki/supported-models）

4.topic，就是bag文件中相机发布图像的topic话题

(1)realsense d435i可以直接读取相机的内参

roslaunch realsense2_camera rs_camera.launch //打开相机节点

rostopic echo /camera/color/camera_info //查看相机内参

（2）标定流程

a.准备标定板和标定板的参数文件

1>可以自己生成：

       安装依赖：sudo apt-get install python-pyx（较重要）

　　依次执行：rosrun kalibr kalibr_create_target_pdf --h

　　rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.055 --tspace 0.3

　　--type apriltag 标定板类型
　　--nx [NUM_COLS] 列个数
　　--ny [NUM_ROWS] 行个数
　　--tsize [TAG_WIDTH_M] 二维码方格长度，单位m
　　--tspace [TAG_SPACING_PERCENT] 小方格与二维码方格长度比例

      执行完上述命令会生成想要的pdf，但还要自己编写yaml文件，如下：  

　　target_type: 'aprilgrid'    #gridtype
　　tagCols: 6                      #number of apriltags
　　tagRows: 6                    #number of apriltags
　　tagSize: 0.02                 #size of apriltag, edge to edge [m]，打印出来要亲自拿尺子量一下tagsize，进行改正，这里yaml和生成时的数据不一定一致
　　tagSpacing: 0.3             #ratio of space between tags to tagSize 

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

2>直接在官网下载，然后根据打印出来的实际数据进行修改

https://github.com/ethz-asl/kalibr/wiki/downloads，下载Aprilgrid 6x6 0.8x0.8 m (A0 page)即可，会得到需要打印的pdf和对应的yaml文件

yaml文件需要根据A4纸打印出来的实际尺寸进行修改，即主要修改tagsize这个数值，tagsizeing是等比例缩小或放大的，故不用改

b.录制camera标定数据：rosbag

为了查看录制照片的好坏，可以通过ROS中的Rviz界面查看具体命令如下：分别在两个终端实现。

　　roslaunch realsense2_camera rs_camera.launch//打开相机节点

　　rviz，选camera_link,add topic->camera->color->image_raw//打开可视化窗口

新开一个终端，将图像频率降低为4HZ，这里可以用throttle方法，不会出错，并发布新的topic，不会修改原topic：

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

 开始录制rosbag包，最好新建一个文件夹，然后在当前终端输入以下命令：

       mkdir bagfiles

　　cd bagfiles rosbag

　　record -O camd435i /color                 # rosbag record [相机话题] -O [保存的bag名字]

录包的时候可以实时打印话题color输出的信息：

　　rostopic echo /camera/color/image_raw

录取bag个人经验：
1、录取时长最少3分钟。--bag-from-to 25 150 最少120s，图片数量500张左右。
2、录取过程中要缓慢移动，倾斜。标定板位置最好出现在rqt_image_view（这也是前面提前打开rviz的目的） 视野的各个位置

其中-O表示设置的bag包名字，/color为录制的话题的名称。ctrl+c结束录制，bag文件会自动保存到录制时所在的目录

c.标定相机

rosrun kalibr kalibr_calibrate_cameras --bag~/kalibr_ws/bagfiles/camd435i.bag --target ~/kalibr_ws/bagfiles/april_6x6_80x80cm.yaml --topics /color --models pinhole-radtan --show-extraction --bag-from-to 5 105

--target //棋盘格yaml参数文件路径 --bag //录制rosbag包路径 --models //相机模型 --topics //话题名称

标定结果保存在kalibr_ws目录下，生成了三个文件

***.yaml //主要作用为了后期imu+相机联合标定

***.pdf //以图的方式显示效果

***.txt //含有相机的内参以及重投影误差（这个是给我们看的，尤其是重投影误差，小于一个px即可）

（二）标定imu

 a.配置realsense的launch文件，在realsense-ros/realsense2_camera/launch文件夹下，修改配置参数

 这样做的目的是将accel和gyro的数据合并得到imu话题，如果不这样做发布的topic中只有加速度计和陀螺仪分开的topic，没有合并的camera/imu 话题。linear_interpolation也可以换成copy

b.标定imu时需要配置imu_utils和code_utils,下载链接为

https://github.com/gaowenliang/imu_utils

https://github.com/gaowenliang/code_utils

　　mkdir ccalibration_ws
　　cd calibration_ws
　　mkdir src
　　git clone https://github.com/gaowenliang/imu_utils.git
　　git clone https://github.com/gaowenliang/code_utils.git
　　cd ..
　　catkin_make
　　source devel/setup.bash

c.编写d435i_imu_calibration.launch文件，在imu_utils/launch文件夹下

　　<launch>

　　<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen"> <param name="imu_topic" type="string" value= "/camera/imu"/>

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

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

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

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

　　</node>

　　</launch>

标定imu时的launch文件记得修改max_time_min对应的参数，默认是120，意味着两个小时，要是你的ros包里的imu数据长度没有两个小时，就进行不下去，始终停留在wait for imu data这里。

标定结果保存在 imu_utils/data/文件夹下

d.具体步骤

step1：进入realsense-ros包，开启d435i，发布IMU话题（顺便打印imu话题：rostopic echo /camera/imu）

　　记得 source ./devel/setup.bash（不然不出现imu的话题）

　　roslaunch realsense2_camera rs_camera.launch

step2: 录制imu数据包

　　rosbag record -O imu_calibration /camera/imu

step3: 运行校准程序（就是C步骤的launch文件）

　　roslaunch imu_utils d435i_imu_calibration.launch（校准程序会显示wait for imu data，等着在线数据或是回放数据）

step4:

　　回放数据包 rosbag play -r 20 imu_calibration.bag

step1->step3同时进行可以实现在线标定，完成后直接生成标定结果

然后将Acc和Gyr的第一组平均数据拷贝到kalibr对应的imu.yml文件中，用于后续联合标定

（三）相机和imu联合标定

流程：

1、根据前面的相机和imu标定结果建立camchain.yaml文件和imu.yaml文件

在imu_utils里的data文件夹，完成imu标定时时会多出d435i_imu_param.yaml文件，圈红部分即为需要的数据，而相机标定的yaml文件在Kalibr_ws里的bagfiles里，上图圈红部分是多出来的，我通过多个帖子比对，大概是默认的，在保留原来相机标定yaml文件数据的基础上，直接copy进去。这样完成了camchain.yaml文件和imu.yaml文件的建立。

2、将图像频率降低为20HZ，imu频率设置为200Hz,这里可以用throttle方法，不会出错，并发布新的topic，不会修改原topic：

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

rosrun topic_tools throttle messages /camera/gyro/image_info 200.0 /imu

3、录制rosbag包（期间可以通过命令查看是否将两个命令的数据都录制上了，rosbag info /home/messi/kalibr_ws/bagfiles/dynamic.bag）

rosbag record -O dynamic /color /camera/imu（确保录制的话题和发布的要一致，不然会录制不上，可以用rostopic list及时查看）

4、标定

rosrun kalibr kalibr_calibrate_imu_camera --bag ~/kalibr_ws/bagfiles/dynamic.bag --target ~/kalibr_ws/bagfiles/april_6x6_80x80cm.yaml --cam ~/kalibr_ws/bagfiles/camd435i-camchain1.yaml --imu ~/kalibr_ws/bagfiles/imu.yaml  --show-extraction --bag-from-to 50 200

（四）结果评定

kalibr标定多目后会生成标定报告以及标定出的外参值，标定报告会直接给出相机坐标的空间位置示意图，结合标定结果和实际位置比较可以较为直观的判断结果是否可靠；另外就是看重投影误差，这个值越小越好，我的在0.15以下已经是很好的了。内参数的话也可以关注一下，我使用的d435i，标定后双面的内参基本一样，不过和自带/camera_info还是有所区别的，但是不是很大。

 参考：https://blog.csdn.net/qq_36170626/article/details/102998942（收费）