0. 前言

在讲完激光SLAM和视觉SLAM后，个人感觉目前对这两块的基础剖析已经较为完善了。这一次也想借着这个机会来写一下外参标定相关的东西。因为对于SLAM而言，在实现多传感器融合前，就需要对每个传感器进行标定，以便于融合的精确。
当一个车上装了多个/多种传感器，而它们之间的坐标关系是需要确定的。这个工作可分成两部分：内参标定和外参标定，内参是决定传感器内部的映射关系，比如摄像头的焦距，偏心和像素横纵比（+畸变系数），而外参是决定传感器和外部某个坐标系的转换关系，比如姿态参数（旋转和平移6自由度）。内参这一块作者已经写了一系列文章来介绍相机的标定方法。这里也给出一个链接。这一篇文章我们主要来讲一下外参的标定。

1. 手眼标定

在SLAM领域中，目前不仅仅局限于单个传感器的建图，目前正在向GPS/IMU和摄像头或者激光雷达的标定，雷达和摄像头之间的标定这样多传感器融合的趋势发展。而手眼标定是一个被标定方法普遍研究的，一定约束条件下的问题：可以广义的理解，一个“手”（比如GPS/IMU）和一个“眼”（激光雷达/摄像头）都固定在一个机器上，那么当机器运动之后，“手”和“眼”发生的姿态变化一定满足一定的约束关系，这样求解一个方程就可以得到“手”-“眼”之间的坐标转换关系，一般是AX=XB形式的方程。对于手眼标定而言，其系统分为两种：eye in hand，眼在手上，相机移动以及eye to hand，眼在手外，相机固定

1.1 眼在手上

图例说明

• {b}：base基座标系
• {g}：gripper抓手坐标系
• {t}：target标定板坐标系
• {c}：camera相机坐标系

矩阵变化

上式中(1)和(2)是指代了在第一个时刻和第二个时刻的坐标系。通过坐标系变换我们可以获得AX=XB形式的问题解决思路。

1.2 眼在手外

图例说明

• {b}：base基座标系
• {g}：gripper抓手坐标系
• {t}：target标定板坐标系
• {c}：camera相机坐标系

矩阵变化

1.3 求解AX=XB

这里我们找到了一个博主使用SVD对矩阵的求解
这里的$A$，$B$和$X$都是$SE(3)$中的元素，即一个坐标系到另一个坐标系的位姿变换。具体来说无论是$A$，$B$还是$X$，都可以写作$\left[ \begin{matrix} R & t\0 & 1\end{matrix} \right]$的形式，其中$R$是旋转矩阵($SO(3)$群中的元素，满足正交且行列式为1)，$t$是任意一个实数列向量。写开来就是

也就是等价于

我们很容易发现，如果我们先解方程(2)，把 $R_X$ 解出来，那么方程(3)对 $t_X$来说就是一个线性方程了，我们可以直接通过最小二乘法求出$t_X$。实际上在绝大多数情况下我们都是用这种两部法，先求 $R_X$，再求 $t_X$

我们主要看(2)。通过硬件上的测量，我们可以得到许多组 $R_A$和$R_B$ ，也就是说我们其实要解这样一个方程组：

其中${ R_{A1},R_{A2},….,R_{An}},{R_{B1},R_{B2},…,R_{Bn}}$都是已知的旋转矩阵， $R_X$ 是一个未知的旋转矩阵。
之后即可通过奇异值分解来构建线性问题，并通过SVD最下二乘法得到$t_X$

总结一下总体步骤就是

2. 传感器外参标定

在自动驾驶中，传感器与传感器之间的外参标定要么是机械结构赋予的粗略值，或者就是通过联合标定的形式实现外参的确定。这一类外参标定的手段以及策略能够使开发者更轻易地实现传感器的标定工作。在不同传感器数据融合的过程中，数据在不同坐标系下的转换都需要使用2个坐标系的外参，通常为旋转矩阵R和平移矩阵T。

idar-imu外参标定工具lidar_align

https://github.com/miracle629/lidar_align这个项目是基于瑞士苏黎世联邦理工大学———自动驾驶实验室再次开发的工作，其实现的步骤是：

1. 首先$(RT)^{-1}$是imu初始坐标系，imu做积分可获得任一时刻相对初始位姿的位姿变换矩阵C,则$(RT)^{-1}C(RT)$表示此时刻激光雷达原点位姿在世界坐标系中的表示，假设某一点在激光雷达坐标系下的表示为$S$，则其在世界坐标系下的绝对坐标为$(RT)^{-1}C(RT)S$
2. 点云扫描前后2帧有很大一部分点云重合，利用点云配准算法，可以估计前后2帧之间的移动转动，从而将每2帧之间的变换$C_i$估计出来,$C_iS$表示$i$点在世界绝对坐标系下的坐标。
3. 我们使用ICP最近邻迭代算法将2种表示方法描述的同一片点云区域配准，构建并优化最近邻误差，这里不直接对获得的laser坐标系构建优化目标，而对坐标系内的点云构建优化目标，运用了统计误差平均效应，直至收敛，获得RT变换矩阵的估计值，从而获得激光雷达与imu的外参变换矩阵。

lidar-camera的PnP标定方法

这点其实很直观就可以想到，因为lidar提供3D的数据，而camera提供2D数据，在高翔博士的《视觉SLAM十四讲》里，PnP方法是作为视觉里程计里一种估计相机位姿的方法提出的，其中3D点是在世界坐标系下的描述，最终得到每个时刻camera的位姿（当然也是在世界坐标系下的描述），我们只要把世界坐标系设定在lidar坐标系之上，就变成了这里的标定问题，只是这里我们的camera没办法相对于lidar坐标系（即世界坐标系）运动。而现存的开源方案也提供了合适的解决方法：https://github.com/CPFL/Autoware/tree/master/ros/src/sensing/fusion/packages/autoware_camera_lidar_calibrator这里使用二维码给lidar和camera对接完成标定。这里其实变成了内参的标定选择。而博主
miracle629也提出了自己的构想。

lidar-gps的标定方法

这部分相关资料较少，我们找到了环宇的相关资料，从资料中我们可以看到本质上仍然是最小二乘的解法。激光雷达与 GPS 坐标系之间的标定，拟采用激光里程计的方法获得激光雷达局部的轨迹，然后通过激光雷达轨迹与 GPS 轨迹之间进行匹配，从而优化外参。
下图反应的就是东风技术中心的前向激光雷达所建的局部地图以及在建图过程中输出的轨迹，蓝色是激光雷达所输出的轨迹，红色是 GPS+IMU 所输出的轨迹。从图上可以看出，正因为激光雷达与 GNSS 系统存在坐标系不统一的问题，使得对于同一个车辆的运动，两个不同传感器输出的轨迹形状相似，但是存在一个 se3 的变换。这里Apollo给出了一个示例。详情也可以参考这篇文章，通过两条轨迹的计算获得四元数信息

3. 参考链接

https://robot.czxy.com/docs/pcl/chapter05/handeye/
https://blog.csdn.net/Sandy_WYM_/article/details/83503861
https://blog.csdn.net/w_weixiaotao/article/details/107786152
https://blog.csdn.net/miracle629/article/details/87854450