激光雷达与相机的联合标定技术(一)

一、前言

        由于自动驾驶、移动机器人等领域的快速发展，这些任务需要精确的环境建模能力，三维感知逐渐受到人们所关注。相机虽然是迄今为止使用最为广泛的传感器，给计算机提供丰富的方位信息和颜色信息。但是，相机非常容易受到环境光线的干扰，在许多情况下无法正常工作。在三维感知中如果只使用视觉传感器，非常容易造成事故，其解决方案直之一是多传感方式的融合以增强模型感知能力。

        较为普遍的做法是激光雷达和相机传感器的融合。为了充分利用来自不同传感器的信息，需要将视觉图像和激光点云置于同一参考系下。因此需要知道激光雷达坐标系和相机坐标系的刚性空间变换，包括相对旋转和相对平移，组成六自由度的外参。求解该变换关系的过程称为激光雷达和相机联合外参标定(Lidar-camera extrinsic calibration)。

二、研究方向

        联合外参标定理论随着多视角几何和最优化理论等发展已经趋于完善，但是在实际情况下，简单并且无人为干预的自动标定仍然是目前的主要挑战。目前的工作主要集中在两个方向：基于目标(Target-based)的联合标定和无目标(Target-less)的联合标定。

        基于目标的联合标定要求测量者提前设计特殊的标定板，标定板上可以添加特殊的视觉效果如棋盘格等，或者镂空特定的形状。

        无目标的联合标定也是目前的主要研究方向之一，主要利用自然场景中的特定形状进行标定，应用于自动驾驶的行驶过程中对激光雷达和相机的实时标定矫正。无目标的联合标定无需手动标记特征，但是要求提供较精确的初值。

三、前置知识--PnP问题

        PnP问题描述为已知空间中的3D点以及其在相机上的投影，求解相机的位姿变换。具体在SLAM应用中，已经空间中的3D点在绝对坐标系或者在上一帧相机坐标系的坐标，以及其在当前帧的投影，求解当前帧相机相对于上一帧相机的坐标变换。
     
        具体而言，已知在世界坐标系(或上一帧相机坐标系)中存在个三维点、、、，以及其在相机的像平面上的像素坐标、、、和相机的内参矩阵。
     
        求解：相机坐标系和世界坐标系之间的位姿变换。即中的世界坐标系到相机坐标系的旋转矩阵和对应的位移向量。其中，为针孔相机模型，此处不多于赘述，$s$为点的深度。

        需要注意的是，本文中，为世界坐标系中点的三维坐标，为相机坐标系中点的三维坐标，为相机坐标系中点的归一化坐标，为点在相机的像素坐标系下的齐次表示。

        PnP问题的求解方法总的来说分为四大类，包括直接线性变换(DLT)、三点法(P3P)、EPnP以及光束法平差(BA，Bundle Adjustment)。以下为BA法的详细推导。

        在理想情况下，所求解的位姿变换符合下式：
                                                                               
        但是在数据的实际采集过程中，由于噪声等误差的存在，该式不严格成立。因此为求出最优的相对位姿()，构建最小二乘问题，对个观测点的误差求和并使得误差最小化：
                                                                   

         进一步说明上式，误差是相机观测的像素点和世界坐标系中的空间三维点经过位姿变换和针孔相机模型投影得到的像素点，计算时使用非齐次坐标，因此此处误差为二维。

        三维点已经经过特征匹配等方法和像素点对应，由于相对位姿的不确定，计算所得像素点和观测像素点的初始误差会非常大，通过最小二乘不断调整相对位姿，使得投影点与观测点之间误差最小。

        在调整位姿的过程中，根据高斯-牛顿方法，其关键在于导数的求解，即雅各比矩阵，误差项对于优化变量的导数，以确定相对位姿的调整方向，以此线性化误差：
                                                                                         

        对上式，，为特殊欧式群，对加法不封闭。因此引入李代数，将变换矩阵映射为李代数。至此，误差为维，李代数为维，确定雅各比矩阵为维。

        之后利用李代数左扰动模型进行推导，以规避旋转矩阵自身的约束()，使得最小二乘为无约束优化问题。

        首先计算三维空间点投影到像平面的点坐标：
                                                                        

        得到
                                                                                                

        因此，误差可以写成观测值为投影像素值之差：
                                                                            

        通过定义三维点在相机坐标系的坐标，应用链式法则和李代数左扰动模型，可以得到：
                                                                                       

        其中，为误差对投影三维点的导数，根据针孔相机模型：
                                                               

        进一步计算得到：
                                                                        

        为变换到相机坐标系上的点对李代数的导数，由左扰动：
                                                   

        采用非齐次坐标，只取前三维度，因此，表示向量到反对称矩阵的变换。因此得到雅各比矩阵

        对于正常的BA来说，还需要对三维点进行优化，此处联合标定并不涉及三维点优化，因此不赘述。此后使用高斯牛顿方法求解即可。