本文是标定系列解读第三篇，介绍了Camera-Lidar标定，通过对一些基础知识和小细节进行讨论和理论推导，给出了一些可以提升标定精度的改进建议。另外本文还给出了一个简单的开源代码对上述标定原理进行实践，以及提供了一个仿真程序，可以直观感受标定数据对系统可观性的影响。希望能够给大家一些启发。
目录
1. 前言
2. 理论
2.1 基于平面约束的相机激光标定算法
2.1.1 平面约束
2.1.2 2D 激光和相机外参数初始值求解
2.1.3 3D 激光和相机外参数初始值求解
2.1.4 外参数优化
2.2 推论：所有平行的平面提供的约束等价。
2.3 拓展：标定板的边界约束
3. 实践
3.1 代码梳理和上手操作
3.2 仿真代码的特别说明
3.2.1 系统可观性的判断
3.2.2 利用仿真代码验证平行平面提供的约束等价
3.2.2 利用仿真代码指导采集数据：如何充分旋转标定板
4. 结语
5. 参考文献
1. 前言
从理论上看，相机和激光之间外参数的标定原理非常简单，但在实际标定过程中，特别是一个初学者采集数据进行标定时，却发现标定结果非常不理想。如何采集有效的标定数据（何种运动轨迹，如何晃动标定板）对于激光相机标定而言非常重要。
读完本文，你会发现原来采集数据时标定板和传感器之间只做纯粹的平移运动是没有意义的。本文也对相机激光标定的一些基础知识和小细节进行讨论，主要贡献点有三：
1.从理论开始对激光（单线、多线都适用）和相机外参数的标定进行简要推导（公式多但简单），然后给出一些可以提升标定精度的改进建议。
2.给出一个简单的开源代码对上述标定原理进行实践。
3.提供一个仿真程序，可以直观感受标定数据对系统可观性的影响。
2. 理论
通常，标定激光和相机之间的外参数有两类方法：一类是利用 3D-3D 的约束，即利用激光测量的三维激光点 (3D) 和相机测量的标定板三维坐标 (3D) 两者来构建约束；另一类是利用 3D-2D 的约束，即利用激光测量的三维激光点 (3D) 和图像二维特征（2D、点特征、线段特征）来构建约束。
本文主要讲解利用 3D-3D 约束来进行外参数标定的方法，而 3D-2D 这一方法和 PnP 或 PnL 问题类似，这里不做展开。另外，标定过程的通常做法是先利用少量观测求解外参数的初始值，然后利用多帧数据的约束进行最小二乘优化对初始值进行 refine。接下来，将按照这个逻辑对标定算法进行讲解并推荐一些改进的算法。
2.1 基于平面约束的相机激光标定算法
如下图所示，相机可以通过标定板平面的二维码或棋盘格来计算标定板平面在相机坐标系下的表示。同时，激光发出的光束落在标定板平面上（图中红点），利用激光点在激光坐标系下的坐标和平面方程在相机坐标系下的坐标，构建点在平面上的约束从而求解外参数。
2.1.1 平面约束
假设标定板平面在相机坐标系
中的参数为
，其中
是平面的三维法向量，
是相机坐标系原点到平面的距离。平面上的一个三维点在相机坐标系下的坐标为
，点在平面上满足：
假设从激光坐标系
到相机坐标系之间的旋转和平移为
。如果知道激光坐标系中某个激光点
落在标定板上，则通过点在平面上这个约束能够构建关于外参数的方程：
上述方程能够提供一个约束，通过多个这样的约束就能求解外参数。求解时，一个直观的想法是利用 g2o 或者 ceres 等优化工具构建非线性最小二乘进行优化求解。但是对于非线性最小二乘问题，需要知道外参数的一个初始值，如果初始值不准确，则有可能会优化到局部最小值。因此，一个合理的求解流程应该是闭式解提供初始值，对该初始值利用多帧数据进行优化，得到更准确的标定结果。
虽然平面约束对于 2D 激光和 3D 激光而言是一样的，但 2D 激光和 3D 激光闭式求解外参数的方式稍有不同。因为 3D 激光的激光点更多，从而可以直接计算激光点云的法向量，利用这个法向量简化外参数计算流程，下文将详述。接下来，介绍 2D 激光和相机外参数的求解。
2.1.2 2D 激光和相机外参数初始值求解
这里主要参考 2004 年 Zhou 等人的论文【1】，该论文是比较早期的工作，求解思路清晰。论文中估计的参数为从相机坐标系到激光坐标系的变换矩阵，由此激光点从激光坐标系变换到相机坐标系为：
考虑到，激光为 2D 激光，激光束形成的平面不妨假设为 xy 平面，即 
，此时有
，则上述坐标系变换可以写成更紧凑的形式：
由此可以将公式 (1) 也写成紧凑形式：
如果把 3 乘 3 的
矩阵当做新的未知量（ 9 个参数）进行估计，那上述约束就变成了一个线性最小二乘问题。对于单线激光而言，一帧激光可以提供两个有效约束（直线上两个点在平面上即能确定直线在平面上），因此大于等于 5 帧激光（不同姿态）就能得到 10 个以上的约束，来直接求得 9 参数的 . 然后恢复出 :
但是，利用上述方式求得的旋转矩阵并不满足旋转矩阵的性质
。假设期望的旋转矩阵为
，则可以通过计算带约束的最小化 Frobenius norm 问题来估计：
通俗的讲，上面这个方程的含义为：找一个最接近
又满足旋转矩阵质的矩阵作为外参数。对于上述方程的求解，实际上有闭式解，可以参考文献【2, 3】。
正如文献【3】指出的，上述方式求解的精度不高，实际应用过程并不推荐，正如多视角几何中估计本征矩阵
一样，也分为 DLT 算法（直接计算 8 个参数）和其他算法。在这里，优雅的方式应该是直接闭式求解 6 自由度的外参数，即旋转矩阵只用 3 个参数而不是上述方法中先估计 9 个参数。参考文献【4】给出了一种闭式求解 6 自由度外参数的方法，由于估计的参数为 6 ，因此需要的最少激光观测变成了 3 帧（每帧提供两个有效约束）。
2.1.3 3D 激光和相机外参数初始值求解
跟 2D 激光的单线数据仅仅能提供一条线相比， 3D 激光的多线数据能提供点云面的信息。因此，同样是利用激光点在标定板平面上这个约束，3D 激光的约束可以比 2D 激光更多一个。
3D 的激光的点云平面和标定板平面重合，意味着单帧 3D激光提供的有效约束为 3 个（即点云平面上的3个点落在标定板平面上），从而可以用 3 个激光点来构建公式 (1) 的约束。这样两帧激光（6个约束）就能求解外参数了，求解方式可以利用前面 2D 激光所描述的方法（则同样存在 DLT 方法初始值不准的问题）。
这里给出了 3D 激光求解外参数的另一个思路，该方法中，求解的旋转矩阵能自然地满足旋转矩阵的性质：
。将单帧激光的点云平面和标定板平面重合的约束进行简单置换：
公式 (2) 中的两个方程中，第一个方程能提供 2 个有效约束，第二个方程提供 1 个有效约束。虽然公式 (2) 和三个点落在平面上的物理意义一样，但是，公式 (2) 中第一个方程只和旋转矩阵有关，和平移无关。意味着可以先求解旋转矩阵，再线性求解平移向量，从而简化参数估计。
当激光帧数 N 大于等于 2 时，可以求解如下非线性最小二乘问题来计算旋转矩阵：
这里基于参考文献【5】介绍上述问题的通用解法。将公式 (3) 进行展开有：
因此，最小化损失函数 C 转化成最大化：
其中，跟 2D 激光求解时一样，引入一个中间矩阵：
为了求解过程清晰，先不加证明地引入一个引理（后续证明），对于任意的正定矩阵
以及任意的正交矩阵
，下面的不等式成立：
对矩阵 H 进行 SVD 分解得到：
其中，
都是 3×3 的正交矩阵，
是 3×3 的对角矩阵，对角线元素非负。取
, 则有：
说明
是个对称的正定矩阵，这时候借用前面的引理，可知旋转矩阵的唯一解是
。反证法，如果它不是正解，则和正确解之间存在一个增量旋转矩阵
，解为
，这个解并不会使得损失函数 F 取最大，因为：
最后，还需要做旋转矩阵性质的判断，如果
，则说明所求得的正交矩阵 是旋转矩阵。如果
，则说明这个正交矩阵是镜面反射矩阵，不是旋转矩阵。幸运的是，在实际应用中，采集的多帧不同姿态下的激光数据并不会出现镜面反射矩阵的情况。
在旋转矩阵已知的情况下，利用公式 (1) 就可以求解平移向量
，这时候求解只涉及到平移，是个线性最小二乘问题
，求解非常容易。
最后，证明前述引理
，假设
是矩阵
的第 
列，有：
考虑到 Schwarz inequality,
, 有：
由此得证： 
。
2.1.4 外参数优化
通过上述方式计算出外参数的初始值之后，再对采集的
组数据进行联合优化得到最优估计，假设第帧激光有
个激光点落在标定板上，同时第 帧激光对应的标定板平面方程在相机坐标系的表示为
：
优化过程中，对于旋转矩阵 ，可以使用 SO3 的参数化方式，也可以采用四元数的参数化方式。优化方法 (LM算法），雅克比的推导等知识点这里不再赘述，大家可以对照开源的代码自行推导验证雅克比。
2.2 推论：所有平行的平面提供的约束等价。
前面求解过程中，是假设采集的数据足够有效，即采集了不同姿态下标定板和激光数据的数据。那么，如何摆放标定板来采集数据是有效的呢？这里将证明只平移标定板对标定没有意义，标定过程中只需要旋转标定板。推荐阅读参考文献【6】。
平移标定板意味着不同时刻之间的标定板平面是平行的。假设有两个平行平面
，它们在相机坐标系中的参数分别为
和
，另外，假设在激光坐标系下，激光测量了两个平面上任意的一个点
, 
。考虑点在平面上的约束有：
通过简单的变换，可以发现上面两个等式能够相互转换，首先将
这部分从两个等式中移除，然后有：
其中，
和
表示两个平面之间的距离，它们两个方向符号相反可以抵消。
由此证明：所有平行的平面物体提供的约束是等价的。这意味着：采集数据用于标定时，如果只平移标定板来采集数据，那就做了很多无用功。比如不断平移标定板采集了 100 个时刻的数据，那这 100 次的数据仅相当于 1 次有效数据。所以，采集标定数据时，只要不断旋转标定板采集数据就足够了，而不是不断平移。
2.3 拓展：标定板的边界约束
上述标定过程只利用了平面标定板的平面约束来进行标定，也清楚了这个约束对于单次测量而言，其提供的有效约束 (2个或者3个) 不足以完成单帧标定，而采集多帧数据又容易由于采集轨迹不充分，导致外参在某些维度标定不好。
因此，如果标定板一次就能够提供 6 个以上的约束，那意味着单帧数据就能完成标定，并且多约束的加入能够提升标定精度。这方面的工作有两类方式，一种是改变标定板的形状，比如两个垂直的标定板，或者三个垂直的标定板，V 形标定板等。
另一种是充分利用其他约束，比如激光点落在标定板边缘直线上（标定板边缘的直线方程以及哪个激光点落在上面是可以计算出来的）。这里不作详细的推导和解释，仅推荐两篇2018年的经典论文，见参考文献【6】和参考文献【7】。论文【6】是利用标定板的边缘直线约束来标定3D激光，论文【7】对2D激光的多种标定板的有效约束进行了充分讨论。
3. 实践
根据上述的理论推导，本文完成了一个简单的 2D 激光和相机之间外参数的标定代码：https://github.com/MegviiRobot/CamLaserCalibraTool。
3.1 代码梳理和上手操作
关于数据的采集，标定板的制作，标定的流程以及标定结果等上手操作细节在项目的 README.md 里已经进行了详细说明，这里对系统中的几个主要文件进行简单梳理，帮助大家更快熟悉代码。
大家关注的外参数的代码大多都在 src 文件夹里的 LaserCamCalCeres.cpp，其中：
- CamLaserCalClosedSolution() ：闭式求解外参数初始值。
- CamLaserCalibration() ：采用非线性最小二乘对初始值进行优化。
另外，比较重要的为 main 文件夹中的三个主程序：
- kalibratag_detector_node.cpp : 相机利用二维码或者棋盘格估计标定板平面和相机之间的姿态。
- calibr_offline.cpp : 处理激光数据（获取在标定板上的激光点云），读取事先处理好的相机姿态，利用这些数据构建平面约束进行外参数求解。
- calibr_simulation.cpp : 生成仿真数据，验证系统参数是否可观。后面小结讲着重说明。
3.2 仿真代码的特别说明
仿真代码可以帮助大家理解采集数据的有效性对标定结果的影响，对理解系统辨识（系统参数是否可观）非常有意义，因此在这里进行特别说明。
代码中的 GenerateSimData() 函数主要用来生成仿真数据，原理为利用激光射线和标定板平面相交，从而得到激光点在标定板上的三维坐标，通过不断变换标定板的姿态，得到多组有效数据（激光点云+标定板在相机中的姿态）用来完成标定。
其中标定板和相机之间的姿态保存在变量
中，代码中采用利用随机数的方式来生成标定板姿态，并可以通过对标定板参数进行不同的设置来对系统的可观性进行验证。
3.2.1 系统可观性的判断
熟悉 VIO(visual inertial odometry) 系统也会熟知可观性问题。实际上，系统的可观性在线代控制理论中有专门论述，涉及到可观性矩阵等知识点，但是那套从线性系统出发的理论可能容易让大家迷惑。这里会从大家熟知的非线性优化求解的角度来简单描述可观性问题。
关于状态量参数是否可观的一个简单解释是：如果改变状态向量
的值，系统对应的观测函数
不发生变化
，则说明系统在某些维度上不可观。直观的理解就是，系统有无数个解从而使得状态量无法辨识。在非线性最小二乘问题中，通过构建误差函数利用高斯牛顿算法进行优化求解时，通常需要求解如下 normal equation：
其中
是残差对状态量求导的雅克比矩阵，
是利用约束构建的残差函数。如果系统求解过程中 矩阵不满秩，假设其零空间为
，有
，意味着求解存在多个解：
即系统有无穷多个解 
使得残差最小，零空间
的基底对应不可观的方向。所以可以通过判断 矩阵的秩来判断系统的可观性，比如 为 6 维的方阵，而秩等于 4，则意味着零空间维度为 2 ，有两个不可观的方向，这个结论在后面将用来指导采集数据。
那么，如果你熟悉 VIO，自然要问既然线性系统中的可观性矩阵和优化方法中的 H 矩阵都能够判断系统可观性？这两者之间存在什么样的联系呢？这里不再展开推导，推荐阅读参考文献【8】，读完第二章你将豁然开朗。
3.2.2 利用仿真代码验证平行平面提供的约束等价
为了生成平行的标定板平面，只需要把代码中的 
设置成常数，如单位矩阵。这时，标定板只有平移，意味着生成的所有数据里标定板平行。编译并运行仿真程序后会有如下信息：
由于估计的外参数为 6 维，其中 H 矩阵的奇异值有 4 个接近 0，意味着只产生了两个维度的有效信息。这和之前的推论结论一致，即多帧数据中平行的标定板产生的约束等效于一帧，而单线激光落在平面上的有效约束为 2 。
另外利用数值的方法计算了 H 矩阵的零空间基底，从图中可以看到最右边两列的两个基底，它们是对应平移 x,y 不可观，而左边两列对应的的两个基底为旋转矩阵角度的两个维度不可观。不可观基底对应的方向表示外参数对应的维度无法求出正确解。
3.2.2 利用仿真代码指导采集数据：如何充分旋转标定板
既然采集数据时只平移标定板不行，那是否意味着简单旋转一下标定板就可以了呢？比如标定板只绕着 y 轴旋转。这里可通过设置不同的旋转矩阵去验证 H 矩阵是否有零空间。通过简单修改代码（注释或反注释设置旋转矩阵的那几行代码），运行后会发现：标定时需要充分旋转 x 和 y 两个轴，标定过程中标定板只旋转一个轴一样会存在零空间基底。
这意味着拿着标定板旋转时，需要充分旋转 roll 和 pitch。更直白一点，假设在长方形的标定板中心画了个十字线，那请绕着十字线的两个轴充分旋转，比如绕着竖轴旋转，然后还要绕着横轴旋转。
4. 结语
至此，相机和激光标定从理论到实践就基本完成了。本文抛砖引玉，介绍了比较基础的标定原理，开源了相应的代码（包括求解代码、仿真代码、debug 验证代码），大家可以进一步改进和拓展（支持边界约束、V 形标定板等）。
最后，这种标定相机和激光外参数的方法估计可以有效避开两个传感器之间的时间延时问题，因为两个传感器都是静止的，晃动标定板时每摆好一个 pose 稍微静止几十毫秒就能避免传感器延时的问题。
作者简介
贺一家，中科院自动化所博士毕业，目前为旷视研究院 SLAM 组研究员，研究方向为基于多传感器融合的 SLAM，结构化特征 SLAM 等。乐于分享和开源，对机器人、AR 等领域有浓厚兴趣。
6. 参考文献
- Qilong Zhang, Extrinsic Calibration of a Camera and Laser Range Finder (improves camera calibration)，http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.80.7118&rep=rep1&type=pdf
- On Closed-Form Formulas for the 3D Nearest Rotation Matrix Problem, http://www.iri.upc.edu/files/scidoc/2288-On-Closed-Form-Formulas-for-the-3D-Nearest-Rotation-Matrix-Problem.pdf
- Finding the Nearest Orthonormal Matrix, http://people.csail.mit.edu/bkph/articles/Nearest_Orthonormal_Matrix.pdf
- A Minimal Solution for the Extrinsic Calibration of a Camera and a Laser-Rangefinder，https://home.deec.uc.pt/~jpbar/Publication_Source/pami2012.pdf
- K.S. Arun, Least Squares Fitting of Two 3-d Point Set, http://post.queensu.ca/~sdb2/PAPERS/PAMI-3DLS-1987.pdf
- Automatic Extrinsic Calibration of a Camera and a 3D LiDAR using Line and Plane Correspondences，https://www.cs.cmu.edu/~kaess/pub/Zhou18iros.pdf
- Wenbo Dong, A Novel Method for the Extrinsic Calibration of a 2D Laser Rangefinder and a Camera，https://arxiv.org/pdf/1603.04132.pdf
- B.T. Hinson, Observability-based guidance and sensor placement, https://digital.lib.washington.edu/researchworks/handle/1773/27387
