在之前的这篇博客中，我们介绍了D435i的ROS接口的基本使用，同时在这篇博客中也介绍了利用Kalibr进行相机标定的基本步骤。之前也预告过，会将两者结合，利用Kalibr对手上的D435i进行实际的标定，这篇博客主要内容就是这些。还是和前面两篇类似，不涉及过多的理论，而是更关注于一些具体的方法和步骤、注意事项。本篇博客主要分为两部分：相机标定和相机-IMU标定。内容参考Kalibr官方Github仓库的Wiki，更详细内容可参考官方文档。
1.相机标定
(1)相机的相关准备
在进行正式标定之前，需要进行软硬件方面的准备，下面简单列出：
软件方面：
- Linux系统，例如Ubuntu 16.04
- ROS环境，例如ROS Kinetic
- Kalibr工具
硬件方面：
- Intel Realsense D435i相机一台
- USB Type-C转Type-A连接线一根(连接D435i和电脑)
- 标定板一个，例如打印好的AprilTag或棋盘格
(2)相机的Ros Bag录制
a.启动RealSense节点(相机)
由于Kalibr是基于ROS Bag进行标定的，因此，一般而言，我们需要事先录制好一段ROS Bag，然后传给Kalibr节点进行标定。首先，我们将D435i连接到电脑上，然后打开一个新终端，输入roslaunch realsense2_camera rs_camera.launch启动RealSense的Camera节点，正常启动完成如下所示。
注意：如果出现找不到ROS节点的情况，请确认其所在的工作空间是否被加入到~/.bashrc里并激活了。如果没有，则添加并激活，就OK了。这个问题在之前的ROS笔记里有提到过，是个很常见的问题。另外插一句题外话，Ubuntu下打开新终端的快捷键是Ctrl+Alt+T，可以记一下，十分方便。
b.可视化单目影像
正常启动节点以后，我们可以利用rqt_image_view工具来查看影像流。新开启一个终端，并输入rqt_iamge_view，选择要查看的Topic，比如D435i的单目相机影像/camera/color/image_raw，如下所示。
这个窗口可以暂时先不用关，后续我们在录制包的时候可以观看内容。
c.影像Topic重新发布
然后，我们需要做的是，将传感器的Topic发送速率“转换”一下，因为在Kalibr的官方文档中，对于相机标定，推荐的速率是每秒4帧左右(FPS=4)，而D435i默认是30帧每秒，所以需要转换一下。转换也非常简单，使用ROS自带的topic_tools即可。新打开一个终端，然后在其中输入rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /cam0/image_raw，如下。
这里简单解释下参数的含义，rosrun topic_tools throttle messages是固定写法，没什么好说的，后面一个参数是源Topic，下一个参数4.0表示的是重新发布的频率，最后一个参数是新发布的Topic的名称。这样我们就可以做到在不改变原始Topic的情况下重新发布一个满足我们要求的Topic了。
d.影像包录制
发布了新的Topic以后，下面终于可以进入录包的阶段了。我们将打印出来的标定板拿好，然后在相机前面来回运动即可。如何运动，简单来说就是先绕相机的各自三个轴，每个轴旋转三次，然后再沿着相机的三个轴，每个轴平移三次，基本就可以了。录包我们使用ROS自带的工具，新打开一个终端，输入如下内容rosbag record /cam0/image_raw即可，如下。
同样，这里简单解释一下含义，record表示录制，/cam0/image_raw表示你要录制的Topic。停止录制按Ctrl+C终止即可，录制的文件会保存在当前终端的路径下，文件名以日期形式保存，后缀名是.bag。此外，我们可以通过rosbag info来查看ROS Bag的相关信息，如下图所示。
至此，我们的包就录制完成了。
(3)配置Kalibr的标定板参数
对于相机标定而言，你需要提供一个描述标定板信息的yaml文件。你可以直接照着Kalibr给出的示例文件修改即可，如下。
主要是包含一些描述标定板类型、行列、大小的信息，结合注释说明，根据自己的实际情况修改即可。
(4)启动Kalibr进行相机标定
由于我的Kalibr所在的工作空间和RealSense节点所在的工作空间不一样，所以需要切换到Kalibr所在的工作空间才可以运行。切换方法也很简单，在终端中输入gedit ~/.bashrc，找到Kalibr空间路径取消注释并把原来的路径注释掉即可，如下图所示。
修改好后保存，并source ~/.bashrc即可。切换回来也是同样的道理，非常简单。注意，如果你的Kalibr和RealSense在同一个工作空间，自然就不必这么麻烦了。
在终端中输入如下命令(注意文件名、Topic名等参数换成你自己的)，启动Kalibr节点：
kalibr_calibrate_cameras --target april_6x6.yaml --bag 2020-09-25-18-07-59.bag --bag-from-to 5 80 --models pinhole-radtan --topics /cam0/image_raw
简单解释各参数内容。target就是刚刚提到的准备好的描述标定板的yaml文件，bag自然不用说，就是刚刚录制好的包，bag-from-to是指选取包中从某一时刻到某一时刻的数据。之所以要这样做是因为在开始和结束的时候，相机有可能会出现剧烈的晃动，不利于标定，可以不用。models就是你指定的相机成像和畸变模型，这里是pinhole成像模型，radtan畸变模型。topics就是包里的影像的Topic的名字。如果有多个相机的话，就依次在models和topics后面对应写出来就OK了。
以上参数设置好之后，就可以帅气的敲一下Enter键，等待结果就可以了。标定过程如下所示。
标定完成后，你会得到三个文件，以txt和yaml表示的标定结果，以及一个pdf报告。至此，利用Kalibr对D435i单目相机的标定就完成了。yaml文件的内容如下所示。
包含内参、畸变参数等内容。
2.相机-IMU联合标定
由于相机-IMU标定和上面介绍过的相机标定基本是一样的，所以这里就不再详细展示相同的中间过程的截图了。需要注意的是，相机-IMU标定会用到上面相机标定的yaml结果文件，所以一般情况下做相机、IMU联合标定之前需要先将相机单独标定好。说一句，Kalibr支持多相机和IMU的联合标定，并非只能一个。
(1)相机-IMU相关准备
在进行正式标定之前，需要进行软硬件方面的准备，与前面相机标定一模一样，不过为了本部分的完整性，下面还是简单列出：
软件方面：
- Linux系统，例如Ubuntu 16.04
- ROS环境，例如ROS Kinetic
- Kalibr工具
硬件方面：
- Intel Realsense D435i相机一台
- USB Type-C转Type-A连接线一根(连接D435i和电脑)
- 标定板一个，例如打印好的AprilTag或棋盘格
(2)相机-IMU的Ros Bag录制
a.启动RealSense节点(相机-IMU)
启动方法和相机标定一样，打开终端，输入roslaunch realsense2_camera rs_camera.launch启动RealSense的Camera节点。这里需要注意的是，如同在这篇博客中提到的，默认情况下D435i是关闭陀螺仪和IMU的，因此在节点启动前，我们需要手动在launch文件中打开对应选项，如下图所示。
当然，如果已经打开了，这一步就不需要了。
b.可视化相机影像
正常启动节点以后，我们可以利用rqt_image_view工具来查看影像流。新开启一个终端，并输入rqt_iamge_view，选择要查看的Topic即可。这个窗口可以暂时先不用关，后续我们在录制包的时候可以观看内容。
c.相机与IMU的Topic重新发布
与前面类似，对于相机-IMU标定，官方文档给出的建议帧率是：相机20FPS，IMU200FPS，因此我们可以重新发布两个Topic。使用ROS自带的topic_tools，新打开两个终端，然后分别在其中输入如下内容：
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /cam0/image_raw
rosrun topic_tools throttle messages /camera/imu 200.0 /imu0
参数的含义见上面的内容。或者也可以先录，录完包之后再用这篇博客提到的一些工具脚本处理。
另外，这里需要注意的是，对于D435i而言，加速度计数据对应的Topic是/camera/accel/samples，陀螺仪对应的是/camera/gyro/samples。而Kalibr接收的IMU数据其实对应于将accel和gyro整合到一起的IMU数据，而不是单独的/camera/accel/samples或/camera/gyro/samples的数据。所以千万不要弄错了，只把某一个数据当作是IMU数据了(当然，我也试过，如果只传入accel数据也能标定出结果，但没有比较过精度如何，感兴趣可以试一试)。如何整合加速度计和陀螺仪的测量见这篇博客。
我在一开始学习的时候，参考的那篇博客(就是参考资料的第四个)里就写错了，导致我标定不出来结果，还以为是自己哪里做的不对，后来才发现用的都不是IMU的数据。所以不得不吐槽一下，网上博客的质量真的是参差不齐，很多很明显错误的信息就白纸黑字写在那里误导别人。不过大家可以放心，我一向对我自己博客的内容严格把关，贴出来的命令、代码一定都是我自己测试过可以运行才会放出来，不误导别人。当然，如果有一些是因为知识水平有限而犯的错误还请大家多多指教。
d.相机与IMU的包录制
这里和相机标定不同的是，我们将标定板固定在某处，让相机运动。这也非常好理解，因为是要标定IMU，如果你都没有动，加速度都为0，还怎么标定呢？而至于运动方式则是和上面相同，可以简记为“三转、三移”。录包我们使用ROS自带的工具，新打开一个终端，输入如下内容rosbag record /cam0/image_raw /imu0即可。这里，我们录制了两个Topic，分别是单目的影像和IMU的观测值。同样的，我们可以通过rosbag info来查看ROS Bag的相关信息，如下图所示。
至此，包就录制完成了。
(3)配置Kalibr的标定板、相机和IMU参数
对于相机-IMU标定而言，你除了需要提供一个描述标定板信息的yaml文件，还需要额外提供两个文件：描述相机内参的文件camchain-xxx.yaml(从相机标定步骤得到)以及IMU的参数文件(详情参考这篇官方文档，主要包含噪声密度和随机游走两个内容)。你可以直接照着Kalibr给出的示例文件修改，或者不确定的部分就用他的也可以。
(4)启动Kalibr进行相机-IMU标定
类似的，在终端中输入如下命令(记得把对应内容改成你自己的)启动相机-IMU标定节点。如何切换工作空间这里略过，可参考上面。
kalibr_calibrate_imu_camera --target april_6x6.yaml --cam camchain-2020-09-25-18-07-59.yaml --imu imu_adis16448.yaml --bag 2020-09-29-13-14-03.bag --bag-from-to 5 90
这里再简单解释下参数。target用于描述标定板，cam用于描述相机的内参等信息，imu用于描述IMU噪声等信息，bag是录制的包，bag-from-to是起始和终止时间。
响亮的按下回车键，相机-IMU标定就开始了，如下所示。
标定完成后，你会得到四个文件，camchain-imu和results-imucam表示的都是相机和IMU之间的变换关系，也就是我们最关心的内容，如下。其余是一些报告内容。
至此，相机-IMU的标定就完成了。
这个地方可能会报个错，说”Optimization failed! Spline Coefficient Buffer Exceeded. Set larger buffer margins!”。字面意思就是样条参数缓存溢出，建议设置更大的缓冲边界。但事实上我设置了更大的边界后依然会报这个错误。后来无意之间发现，这个错误可能和起始和终止时间有关。在报这个错误的时候，我设置的其实和终止时间是10到50秒。但是如果换成其它时间，如20到120秒就没有问题了。所以如果你也遇到这个问题，不妨--bag-from-to命令换个起止时间，可能就好了。
3.小结
上面就是利用Kalibr对D435i进行标定的主要流程了，其实总体而言并不算难。只要按照流程一步步走基本都OK。只是标定的精度高低的问题。其实在上面的标定中，标定板都是简单用打印机打印出来的。但按我导师的话说，要想获得高精度的标定结果，标定板一定要是高精度的，简单打印机打印出来肯定不行。但如何获得高精度的标定板又是另一个故事了，这里也就不说了。
4.参考资料
- [1]https://github.com/ethz-asl/kalibr/wiki/multiple-camera-calibration
- [2]https://github.com/ethz-asl/kalibr/wiki/Multi-IMU-and-IMU-intrinsic-calibration
- [3]https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
- [4]https://blog.csdn.net/qq_36170626/article/details/102998942
本文作者原创,未经许可不得转载,谢谢配合
