激光slam坐标系和视觉slam坐标系对齐，两个slam系统之间坐标对齐，轨迹对齐，时间戳对齐

1. 面临的问题

两个独立的SLAM系统中，常常面临一个问题，那就是一个系统上的某一个pose，对应到另一个系统中是在哪里？

紧耦合的SLAM系统，不存在这个问题，比如激光雷达和相机融合的SLAM系统，它们不存在上述问题！

造成这种关系不确定的最主要原因是：两个系统各自有自己的一个参考系。如果它俩在一个统一的世界参考系下，那么就不存在上述的问题。

我们立马想到的一个办法是，我让两个SLAM系统同时开机运行，那么开机运行那一刻的原点不就是它们的坐标原点了嘛！但是你能保证两个SLAM系统都是一开机运行就能初始化成功吗？对于单目视觉slam，它的世界坐标系是在初始化成功的第一帧。也就是说可能开机运行了一段时间之后，它才出现自己的世界坐标系。而激光SLAM、RGBD SLAM、立体相机SLAM情况又不一样。

所以我们需要一种更通用的方法，来解决两个坐标系的对应问题。

以下分析方法，对于多于两个的SLAM系统也适用！

2. 问题分析

如果用几何来描述这个问题，可以通过下图来表示：

图 1

坐标系$O-xyz$和坐标系$O^{\prime }-x^{\prime }{y}^{\prime }{z}^{\prime }$分别表示两个SLAM系统的坐标原点，只要能找到这两个坐标系之间的变换关系，那么两个坐标系下的位姿就可以通过这个变换关系相互变换。

假设我们已经求出来了$O\to O^{\prime }$的变换矩阵为$T_{O^{\prime }O}$

那么，$O-xyz$坐标系下的某一个位姿$P_O$转换到$O^{\prime }-x^{\prime }{y}^{\prime }{z}^{\prime }$坐标系下可以通过如下公式：
$$P_O^{\prime }=T_{O^{\prime }O}{P}_O\text{\hspace{1em}(0)}$$

反过来，$O^{\prime }-x^{\prime }{y}^{\prime }{z}^{\prime }$坐标系下的位姿$P_O^{\prime }$的到$O-xyz$变换:
$$P_O=T_{O^{\prime }O}^{-1}{P}_O^{\prime }\text{\hspace{1em}(1)}$$

上面介绍的变换是刚性变换，有时可能面临尺度不统一的问题，那么刚性变换就不能满足条件了，必须使用sim3变换。

3. 解法

要求解上述的变换矩阵，只需要找到几对对应点，比如$O-xyz$坐标系下的某一个点，对应找到它在$O^{\prime }-x^{\prime }{y}^{\prime }{z}^{\prime }$坐标系下位置，就这样就形成了一个对应点对，如果能找到多对这样的点对，那么势必是可以求解出这样的变化矩阵$T$。这里我就不卖关子了，通过这种对应关系求解变换矩阵的方法很多，其中一种比较常用的方法是：umeyama algorithm，它实际上就是通过最小二乘的思路推导出来的，它不但可以求解刚性变换，还可以求解sim3变换。

该算法我自己实现了一个版本，如果需要可以参考：align_trajectory

4.工程中具体做法

4.1根据时间戳找匹配

我们的机器人上可能同时安装了激光雷达和相机，它们可以同时开始运行或者先后运行两个独立的SLAM系统，那么此时就需要统一两个坐标系，由于它们刚性连接，所以它们最终必然产生相似的轨迹形状。

暂时是假设两个SLAM系统精度都比较高，或者比较相近，因为只有这样轨迹形状才会很相似。

对于这种情形，可以采用的坐标系统一方法是：按时间戳对齐。

由于激光雷达和相机被固定连接在一起，所以某一个时刻它们必定处在同一个真实空间位置。那么只需要根据时间戳，取出该时刻下两个传感器对应获得的位姿。根据多个时间戳最终就可以获得多对这样的匹配，然后采用umeyama algorithm，就可以求出两个坐标系之间的变换矩阵。

由于时间戳的精度比较高，有可能激光雷达坐标系下的某一个位姿，在当前时间戳下，相机坐标系并没有与之对应的位姿，此就需要根据实际情况扩大时间戳的范围，比方说在当前时间戳前后0.01秒之内的，可以认为是对应的位姿。

4.2 根据空间位置找匹配

此种方式限制性更小一些。

多数情况下激光雷达建图是会比视觉建图更琐碎一些，可能要控制机器人在环境中来回的运动，对于更大的环境，甚至还需要一点儿点儿精细的对环境进行建图，如果此时相机与激光雷达同时运行势必会浪费很多计算资源和空间，可能还会给视觉带来更大的累计误差，所以最好的方式应该是激光雷达先进行建图，然后打开激光雷达的重定位功能，进行实时的重定位，然后相机进行视觉SLAM功能，视觉SLAM当前时刻跟踪出来的位姿，与当前激光雷达重定位获得的位姿就形成了一对对应点，同样的可以获得很多这样的对应点对。进一步还可以根据激光雷达的定位置信度选择定位更准的点。然后根据umeyama algorithm算法求解出来两个坐标系之间的变换矩阵。

如果你使用的是单线激光雷达，那么势必就少了一个维度（高度上）的信息，而相机恢复出来的是三维信息，不过这并不影响，在实际使用umeyama algorithm时，只需要将激光雷达的高度数据设置为0

这种方法不但适用激光雷达与相机，甚至里程计与相机，里程计与激光雷达，相机与相机，激光雷达与激光雷达，只要是能获得重新定位的能力，此种方法理论上就是适用的。

可能你会怀疑这种方法的精度，实际来说这种精度的误差主要还是来自于SLAM本身，或者测量误差，一旦你能很精准的获得匹配关系，这种误差实际上很小的。

我做过一个实验，1400多米长的轨迹，分别使用激光雷达和相机进行SLAM，最终通过时间戳进行轨迹对齐，它们的误差非常小，可能只是因为SLAM精度带来了这种影响。如果你好奇具体做法，可以参考我github上的代码：align_trajectory