多激光雷达手眼标定

手眼标定方法已经有很多博客进行解析，但是都是针对机器人的手（夹爪）眼睛（相机）进行标定。例如：
标定学习笔记（四）-- 手眼标定详解
手眼标定_全面细致的推导过程

本文主要描述多激光雷达应用中如何使用手眼标定的方法进行标定。
假如存在以下问题，在一个小车上存在两个激光雷达，激光雷达A与激光雷达B，须求解激光雷达B到激光雷达A的转换参数${}_B^{A}T$（外参）。

利用手眼标定方法原理如下：

变量定义

如上图中所示，存在一个世界坐标系G，点$P$在世界坐标系中的坐标为$P_G$。
在$t_1$时刻，激光雷达A与激光雷达B分别观测到点$P$的坐标为：$P_{A1}$，$P_{B1}$。
在$t_2$时刻，激光雷达A与激光雷达B分别观测到点$P$的坐标为：$P_{A2}$，$P_{B2}$。

在$t_1$时刻，激光雷达A与激光雷达B分别到全局坐标系$G$的转换关系为：${}_{A1}^{G}T$，${}_{B1}^{G}T$。
在$t_2$时刻，激光雷达A与激光雷达B分别到全局坐标系$G$的转换关系为：${}_{A2}^{G}T$，${}_{B2}^{G}T$。

在$t_1$时刻，激光雷达A与激光雷达B之间的转换关系为${}_{B1}^{A1}T$
在$t_2$时刻，激光雷达A与激光雷达B之间的转换关系为${}_{B2}^{A2}T$

推导过程

在$t_1$时刻，点$P$在世界坐标系中的坐标$P_G$可以通过下式计算：

$$P_G={(}_{A1}^{G}T){(}_{B1}^{A1}T)P_{B1}$$
在$t_2$时刻，
$$P_G={(}_{A2}^{G}T){(}_{B2}^{A2}T)P_{B2}$$

由上述两式可得：
$$P_G={(}_{A1}^{G}T){(}_{B1}^{A1}T)P_{B1}={(}_{A2}^{G}T){(}_{B2}^{A2}T)P_{B2}=P_G$$

由于认为激光雷达A与激光雷达B之间是刚性连接，所以二者之间的变换不随时间进行变化，即：
$${}_{B1}^{A1}T{=}_{B2}^{A2}T{=}_B^{A}T$$
则上式变换为：
$${(}_{A1}^{G}T){(}_B^{A}T)P_{B1}={(}_{A2}^{G}T){(}_B^{A}T)P_{B2}$$

两边同左乘${(}_{A2}^{G}T{)}^{-1}$，并同时右乘$(P_{B1}{)}^{-1}$，转换为下式：
$${(}_{A2}^{G}T{)}^{-1}{(}_{A1}^{G}T){(}_B^{A}T)={(}_B^{A}T)P_{B2}(P_{B1}{)}^{-1}$$

假设${(}_B^{A}T)$为要求解的变量$X$，系数为$A={(}_{A2}^{G}T{)}^{-1}{(}_{A1}^{G}T)$与$B=P_{B2}(P_{B1}{)}^{-1}$。
则上式转换为：
$$AX=XB$$
这就是我们经常所说的手眼标定推导出的形式了。但是，这和我们的激光雷达轨迹有什么关系呢？

接着，对系数$A$与$B$分别进行转换，过程如下：

对式$A$，代入${}_{A1}^{G}T={(}_{A2}^{G}T){(}_{A1}^{A2}T)$得：
$$A={(}_{A2}^{G}T{)}^{-1}{(}_{A1}^{G}T)={(}_{A2}^{G}T{)}^{-1}{(}_{A2}^{G}T){(}_{A1}^{A2}T){=}_{A1}^{A2}T$$

对式$B$，分别代入$P_{B1}={(}_G^{B1}T)P_G$与$P_{B2}={(}_G^{B2}T)P_G$得：
$$B=P_{B2}(P_{B1}{)}^{-1}={(}_G^{B2}T)P_G{(}_G^{B1}T{P}_G{)}^{-1}$$
$$={(}_G^{B2}T)P_G{P}_G^{-1}{(}_G^{B1}T{)}^{-1}={(}_G^{B2}T){(}_G^{B1}T{)}^{-1}$$
代入${}_G^{B2}T={(}_{B1}^{B2}T){(}_G^{B1}T)$得：
$$B={(}_G^{B2}T){(}_G^{B1}T{)}^{-1}={(}_{B1}^{B2}T){(}_G^{B1}T){(}_G^{B1}T{)}^{-1}{=}_{B1}^{B2}T$$

由此，可得$AX=XB$可以转换为下述形式：

$${(}_{A1}^{A2}T)X=X{(}_{B1}^{B2}T)$$

式中，${}_{A1}^{A2}T$为激光雷达$A$从$t_1$时刻到$t_2$时刻的位姿变换（也就是轨迹）。同样的，${}_{B1}^{B2}T$为激光雷达$B$从$t_1$时刻到$t_2$时刻的位姿变换。