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

本文主要是介绍激光slam坐标系和视觉slam坐标系对齐，两个slam系统之间坐标对齐，轨迹对齐，时间戳对齐，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

1. 面临的问题

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

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

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

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

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

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

2. 问题分析

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

在这里插入图片描述

图 1

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

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

那么， $O - x y z$ 坐标系下的某一个位姿 $P_{O}$ 转换到 $O^{'} - x^{'} y^{'} z^{'}$ 坐标系下可以通过如下公式：
$P_{O}' = T_{O'O} P_{O} \tag{0}$

反过来， $O^{'} - x^{'} y^{'} z^{'}$ 坐标系下的位姿 $P_O'$ 的到 $O - x y z$ 变换:
$P_{O}= T_{O'O}^{-1} P_{O}' \tag{1}$

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

3. 解法

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

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

4.工程中具体做法

4.1根据时间戳找匹配

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

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

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

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

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

4.2 根据空间位置找匹配

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

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