多传感融合内外参标定（二）：lidar-camera，PnP算法和自动化标定设想

lidar-camera的PnP算法

问题简述

多线激光雷达lidar和相机camera（我又想起了SLAM的派别之争），是无人驾驶里最重要的2种传感器。lidar提供精确的点云数据，可以建立稠密的三维世界地图，便于无人车运动时避障路径规划。但lidar只提供点坐标的信息，略去了颜色、纹理、形状等等特征，所得数据不直观，而camera就很好地弥补了这一点，被称为最接近人类视觉的传感器。可是camera在成像的过程中进行了三维空间的二维投影变换，压缩了一个维度，略去了深度信息。所以这两种传感器是天然的互补。
好了，camera和lidar在返回数据时都是基于自身自带的坐标系，所以lidar-camera的外参标定问题就可以描述成2个坐标系之间的旋转R和平移t。这里再次声明，本系列文章里，使用的$(Rt)$表示完整的6自由度变换，不表示矩阵与向量的点乘，同样$(Rt{)}^{-1}$只表示其反变换。

3D-2D的PnP算法

这点其实很直观就可以想到，因为lidar提供3D的数据，而camera提供2D数据，在高翔博士的《视觉SLAM十四讲》里，PnP方法是作为视觉里程计里一种估计相机位姿的方法提出的，其中3D点是在世界坐标系下的描述，最终得到每个时刻camera的位姿（当然也是在世界坐标系下的描述），我们只要把世界坐标系设定在lidar坐标系之上，就变成了这里的标定问题，只是这里我们的camera没办法相对于lidar坐标系（即世界坐标系）运动。
我们这个系列不是讲数学理论的，所以就简要提一下，PnP的解法有很多种：
1.直接线性法：使用$3\ast 3$矩阵表示旋转时，即问题变为增广矩阵[R|t]中12个参数的估计，每个点提供2个约束方程，6个点可以求解。注意这里R实际只有3个自由度，却用9个参数表示，即旋转矩阵自身有约束，而直接线性法求解时忽略了这个约束，所以估计的矩阵要使用QR分解使得约束重新满足。
2.P3P：当相机参数已知，我们可以通过图像坐标系下像素坐标计算相机坐标下的像素点的3维坐标，无形中得到了3个点，所以本来的6点就减到只需要另外3个点，并且P3P还充分利用了几何约束。
3.BA(Bundle Adjustment)：光束平差法，基于李代数和矩阵微分（可以用梯度下降搜索了），把估计参数分当成一个优化问题，一般使用其他方法解方程得到解析解初值，然后算BA优化一下。
此外还有EPnP、UPnP等等，在OpenCV里使用SolvePnP函数，使用哪种方法只需要改变参数就可以。

现有工具的差劲表现

讲到这里，我们会觉得今天这个博客重点已经将完了，接下来说一说有哪些工具就可以了。其实不然，现成的解决lidar-camera的工具不少，但效果参差不齐，并且效果不稳定，不保证每次都能成功。现存的开源方案例如Autoware自带的工具，连接https://github.com/CPFL/Autoware/tree/master/ros/src/sensing/fusion/packages/autoware_camera_lidar_calibrator，使用二维码，需要特别准备环境的lidar_camera_calibration,连接https://github.com/ankitdhall/lidar_camera_calibration，其他就不列举了。
总之，最后的PnP算法其实并没有太大的出入，制约lidar-camera标定的其实是camera和lidar的预处理，最重要的2个问题：
1.如何将2D和3D的数据点一一对应起来的？
2.camera和lidar都存在将连续的现实空间离散化的处理方法，如何保证选对应的点真的是都在同一个位置上？

改进方案和自动化标定设想

现有方案的方法

面对第一个问题，现有方案无非2种。一是人在图像和点云中分别点取，生成一个数据对，这种方法一般不会选错，但是太耗时间，不利于自动化；二是使用特征法，将点分别用特征检测子检测和特征描述子表示，然后使用算法一一对应，但这里一个是2D特征，一个是3D特征，设计特征和匹配需要非常用心，最好在场景里做特殊布置，减小先期处理的复杂度，但这种方法可以适应自动化标定的发展。
面对第二个问题，大多方案没有做解决，例如图像里角点易单独识别，但点云图中就是没有光束照射到这个角点上（我也很无奈啊），只能取最近的被照射的点。比如下图，我在2D图中选取了壶嘴前面最尖的那个点，但3D点云就只有那个点旁边的2个扫描点，对应选取的点本身就是近似的。

所以，现有方案可以用2个词总结：不精确、太繁杂。

不存在的精确强特征点

是时候表演真正的技术了，本博的重点其实在这里，吼吼！
受圆点阵棋盘相机标定通过拟合圆心求得一个圆心阵列这个方法的启发，我们觉得3D的激光点，拟合球心就可以了。因为球在二维相机里还是呈现圆的形状，所以camera的处理还可以使用现成的拟合圆的算法，lidar数据的处理可以使用拟合球面的算法，这在pcl中容易实现，由于在2D、3D中都很易识别，所以我们称其为强特征。3D的球心的2D映射必然是那个圆的圆心，一定是同一个点，原理上没有问题，那么球心、圆心够精确吗？答案是比我们手选的圆边上的点，球面上的点精确，这里又要提及统计误差平均效应，后面小节我们会单独讲一下，总之，这个点够精确。
我们先看一下我的设计图吧。

自动化标定流程

这个圆点阵只要从前方看就是个圆点阵棋盘，当然我们把凹进去的球面涂成其他颜色，容易识别，而lidar从前方可以将光束投射到一个平面和这些凹进去的球面上，平面拟合和球面拟合在点云处理中都存在很成熟的方法。
至于制作这个标定板，只要有3D模型，3D打印一会儿的事情。为了使标定过程更加自动化，我们可以在这个板后面加上万象锁支撑架，板可以自由移动、升降、旋转，在不同位置呈现不同姿态。再加上几个小电机，写一个控制程序，我们可以在标定时获得空间不同位置、不同姿态的数据。再仔细一想，其实camera内参标定也可以一并进行，666啊。我在这里再挖个坑，由于最近忙找实习、准备工作的事情，没时间填坑，呜呜呜。

统计误差平均效应

我们测量、实验中经常使用多组数据求均值这个思想，很直观的想法。
我们假设在球面上采到的N个数据的各个分量不相关且每个分量自身的维度上均遵守高斯分布（其实这个假设不正确，不会不相关的），那么当通过N个点拟合球心时，误差就会被摊到每个点上，每个点可能都错位一点点，但不会出现某个误差特别大而使数据无法使用。多个数据正的误差和负的误差相互抵消，最差的情况，误差都偏向同一方且一样大小，最后拟合的球心误差也向这个方向偏，大小和每个数据点误差大小相同，然而我们还有高斯分布的假设，实际上球心的误差比直接测得的点的误差要小的多。这个思想也是BA或者说最小二乘法优化的基础。