激光雷达和相机的联合标定

最近在做相机和激光雷达的联合标定，花费了大把力气。

操作系统使用的设备：

Ubuntu18.04
Velodyne VLP16激光雷达
Kinect DK相机

准备工作

相机的sdk安装和ros相关文件的安装：https://blog.csdn.net/qq_27399933/article/details/107356117
可能会出现找不到libk4a1.4-dev的问题，运行下面的指令：

curl https://packages.microsoft.com/keys/microsoft.asc | sudo apt-key add -
sudo apt-add-repository https://packages.microsoft.com/ubuntu/18.04/prod

激光的的驱动和相关ros文件的安装：https://blog.csdn.net/zbr794866300/article/details/99305864

方案一

首先参考的是旷视的标定算法，代码链接附上：https://github.com/MegviiRobot/CamLaserCalibraTool，里面有详细的原理介绍和操作步骤。这里使用的是单线激光，即scan，不是一般用的激光点云图（velodyne_points）。由于当时是使用打印在白纸上的棋盘格做的测试，效果不好，方法应该没有问题。

方案二

参考的是无目标标定方法，代码链接附上：https://github.com/hku-mars/livox_camera_calib，使用的是固态激光雷达livox，使用Velodyne VLP16激光雷达采集的激光点云过于稀疏，重建出来的效果不是很好，在尝试了建图算法提升点云密度后，效果依旧不是很好，决定放弃。

方案三

使用autoware的calibration toolkit，代码链接附上：https://github.com/Autoware-AI/autoware.ai，安装主要参考：https://www.icode9.com/content-3-778900.html，其次参考：https://blog.csdn.net/xz1203/article/details/111474782，博主装的是有cuda的版本，不带cuda的版本编译没有通过。
使用第一个博客在最后编译的时候出现了问题，参考第二个博客更新ros的方法，然后成功了

rosdep update
rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

由于装的是1.14版本的autoware，这个版本的autoware已经吧calibration toolkit给分割出来了，要重新安装calibration toolkit，安装主要参考：https://blog.csdn.net/qq_43509129/article/details/109327157。
这里首先要强调一下标定板的重要性，标定板要尽量平整，然后不反光为好，不然使用calibration toolkit时grab不出来，主要问题就出在cv::findChessboardCorners()检测不到棋盘格，棋盘格的链接附上：棋盘格，博主使用的是A0的尺寸，板子太小的话，不方便后续的圈点云图像工作
这里推荐的一个办法是先用内参工具来测试一下相机是否能正确检测出棋盘格:
1、安装camera-calibration

sudo apt install ros-melodic-camera-calibration

2、安装openni的驱动

sudo apt-get install ros-melodic-rgbd-launch ros-melodic-openni-camera ros-melodic-openni-launch

3、启动openni

roslaunch openni_launch openni.launch

4、启动标定工具：

rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.025 image:=/rgb/image_raw camera:=/camera --no-service-check

这里的square的指的是小正方形的边长（单位为m），这里只是检测一下棋盘格，可以不做修改，注意这里的image部分要和自己图片的节点相吻合，camera的名字部分因为加了no-service-check，所以没啥关系。如果可以检测到棋盘格，并且晃动标定板之后仍然没有问题的话，基本可以确认标定板部分ok。

下面开始使用calibration toolkit工具进行标定：
首先需要启动相机和激光雷达，按照上面准备工作的来就可以，保证在rviz界面可以看见相机和激光雷达的图像，然后开始录制rosbag包：

rosbag record /rgb/image_raw /velodyne_points #录制rgb图和激光点云图

这里可能需要根据自己相机和激光雷达的类型做一定的修改。
拿着标定板在近远距离分别做五组动作，每个持续3-5秒，动作如下（这里借鉴别人的图像）：

5个动作分别为:正向; 下俯; 上仰; 左偏; 右偏。即拿着标定板正向对着相机，下俯，上仰，左偏右偏对着相机，另外，上下左右偏的角度不宜太大，否者标定板的棋牌格可能占有很小的相对面积甚至不清楚，容易导致后面获取不到有效识别的标定板棋牌格数据帧。
录制好rosbag包之后开始标定。
首先播放rosbag包：

rosbag play bagName.bag /velodyne_points:=/points_raw

这里将激光雷达的原始节点名称改为/points_raw，因为在标定功能包的mainwindow函数中，订阅的是/points_raw这个话题，所以需要修改名字，或者修改源码重新再编译一次。

启动之后待数据跑起来后，立马按空格暂停。
然后打开calibration toolkit:

roscore
rosrun calibration_camera_lidar calibration_toolkit

选择相机图像的话题名称

选择Camera->Velodyne

然后再次按空格播放rosbag的包，就可以看到如下的界面

首先修改上方的参数

pattern size表示标定板上每个小长方体的长和宽，我这里是边长为10.8cm的小正方体，所以是$0.108\times 0.108$（单位是m） ，pattern number指的是(每列小长方形的个数-1)$\times$(每行小长方形的个数-1)，我这里是$9\times 7$，所以填$8\times 6$，因为算法不检测最外面一圈的角点，所以要分别减1。
然后调整图片的位置，便于观测，并点击图片右侧的黑色区域，按b键选择浅色背景

这时可能还是看不到激光点云图，再按2键，可以看到类似效果

然后通过快捷键调节点云图，方便看见清晰的棋盘格图像

+、-可以缩放图像大小

通过调节得到这样的一个图像

然后点击右上角的Grab，如果点击Grab没有反应，说明没有识别到这一帧图像中的棋牌格（第一次做标定，在这里卡住了好久，经常按了Grab没有反应，然后发现是棋盘格的问题，所以强烈建议尝试一下上面讲的通过标定内参工具检测棋盘格可行性的方案），继续播放数据，然后又暂停，能看是否能获取有效的的数据帧。如果成功获取数据帧，如下图，就可以在右下框中进行标定，主要是标出标定板的位置，需要注意的是，鼠标放在不同的位置，会有不同的坐标系，最好使得坐标轴垂直标定板的朝外出来，还有就是竖直方向朝上，然后点击鼠标左键选取面积，右键是取消然后重新选取。

重复工作，直至bag包播放结束

标定一副之后，继续播放数据，然后看到合适的场面（标定板的位置比较好时候），暂停数据，然后标定，直到数据包跑完为止。

然后右上角的calibrate键，计算内外参

然后可以点击Project进行投影，会根据计算结果和激光雷达数据生成的图像对应位置，以红色散点表示，如果红色散点分布到标定板上，则说明正确，如果没有，就重新选点。

这里做演示，只Grab了三张图，实际做的时候按照理论，六个位置，每个位置五个动作，三十个最好，不过少一点也没什么关系。
落在标定板上的就是正确标定，如果散点一半在一半不在或者不在的情况都是有问题的，然后对右下标定界面重新标定一下在后再计算验证。

弄完之后没有问题，就点击左上方的“Save”，将结果保存到合适位置，保存过程中下面两个弹窗都选“NO”

保存的文件名要以.yaml结尾。
将我们标定的结果用上，将三维点云数据投影到图像上，此处借助Autoware自带的Calibration Publisher和Points Image节点能进行融合。因此要单独启动Autoware软件。在Autoware/Sensing/中打开Calibration Publisher，并将保存好的内外参文件读取进来

打开Autoware/Sensing下的Point Image节点，一般不用修改，默认就ok

事先运行录制好的rosbag包，开始播放后按空格暂停，播放指令和之前一样：

rosbag play bagName.bag /velodyne_points:=/points_raw

/velodyne_points:=/points_raw不能丢，点击Autoware中的Rviz按钮，直接在终端打开rviz不可以，Panels/Add New Panel内添加ImageViewerPlugin，填写Image Topic，本例为rgb/image_raw，Point Topic，本例为/points_image

然后播放rosbag包就可以看见融合的效果：

如果对的不是很齐，考虑重新标定，至此标定工作完成。
参考博客：https://blog.csdn.net/zbr794866300/article/details/107144682