SLAM算法评估中的轨迹拟合与外参求解

文章同步更新到github pages，欢迎收藏

文章目录

问题描述
数学关系
求解方法
- 求解$_{gp}^{gv}T$
- 求解$_{lp}^{lv}T$
- 交替迭代优化
时间戳对齐
实验结果
参考文献

问题描述

SLAM中常常碰到对齐（align）两个不同设备采集的轨迹的问题。比如通过VICON跟踪获得了一组轨迹，手机通过SLAM算法也获得一组轨迹，要评估SLAM算法的精度，就需要将手机获得的轨迹与作为真值的VICON轨迹对齐。

数学关系

设lpgpT_{lp}^{gp}TlpgpT代表手机（phone）从当前局部坐标系（local）到SLAM算法定义的世界坐标系（global）的变换，即当前手机在SLAM算法定义的世界坐标系下的位姿。

设lvgvT_{lv}^{gv}TlvgvT代表从VICON的当前刚体坐标系到VICON定义的世界坐标系的变换，即VICON跟踪的刚体在VICON世界坐标系下的位姿。

设gpgvT_{gp}^{gv}TgpgvT代表两个世界坐标系之间的变换

设lplvT_{lp}^{lv}TlplvT代表当前刚体坐标系与手机坐标系之间的变换

这几个变换满足：
gpgvT∗lpgpT=lvgvT∗lplvT_{gp}^{gv}T\ *\ _{lp}^{gp}T\ =\ _{lv}^{gv}T\ *\ _{lp}^{lv}T gpgvT ∗ lpgpT = lvgvT ∗ lplvT

其中lpgpT_{lp}^{gp}TlpgpT和lvgvT_{lv}^{gv}TlvgvT均是已知量，多个lpgpT_{lp}^{gp}TlpgpT和lvgvT_{lv}^{gv}TlvgvT各自构成了两条运动轨迹，轨迹中的每一帧记为lpgpTi_{lp}^{gp}T_ilpgpTi和lvgvTj_{lv}^{gv}T_jlvgvTj。gpgvT_{gp}^{gv}TgpgvT和lplvT_{lp}^{lv}TlplvT是待求解的未知量。如果手机与VICON刚体是刚性连接的，在整个运动过程中，gpgvT_{gp}^{gv}TgpgvT和lplvT_{lp}^{lv}TlplvT的值是定值。

求解方法

优化的目标函数为：
min⁡gpgvT,lplvT∑i∥gpgvT∗lpgpTi−lvgvTj(i)∗lplvT∥F2\min_{_{gp}^{gv}T,\ _{lp}^{lv}T} \sum_{i}\Vert\ _{gp}^{gv}T\ *\ _{lp}^{gp}T_i\ -\ _{lv}^{gv}T_{j(i)}\ *\ _{lp}^{lv}T\ \Vert_F^2 gpgvT, lplvTmini∑∥ gpgvT ∗ lpgpTi − lvgvTj(i) ∗ lplvT ∥F2
其中lvgvTj(i)_{lv}^{gv}T_{j(i)}lvgvTj(i)表示在时间戳上与lpgpTi_{lp}^{gp}T_ilpgpTi对应的某一帧。

上式不好直接优化，原因在于

自由度过大，如果旋转用四元数表示，上式要同时优化12个自由量。
约束不足，存在奇异解，比如gpgvT_{gp}^{gv}TgpgvT和lplvT_{lp}^{lv}TlplvT均为零，显然是上式的一个极小解。

因此考虑分部优化。

求解gpgvT_{gp}^{gv}TgpgvT

首先假定lplvT_{lp}^{lv}TlplvT已知，只求解gpgvT_{gp}^{gv}TgpgvT，相当于是寻找一个刚体变换将已知轨迹lpgpT_{lp}^{gp}TlpgpT变换为另一个已知轨迹lvgvT∗lplvT_{lv}^{gv}T\ *\ _{lp}^{lv}TlvgvT ∗ lplvT，并满足最小二乘。这类问题有一个高效的线性解法。

设lvgvT∗lplvT=lpgpT_{lv}^{gv}T\ *\ _{lp}^{lv}T\ =\ _{lp}^{gp}TlvgvT ∗ lplvT = lpgpT，两条轨迹的translation部分分别为lpgpt_{lp}^{gp}tlpgpt和lpgvt_{lp}^{gv}tlpgvt，将轨迹中的所有translation求均值得到质心lpgptˉ_{lp}^{gp}\bar{t}lpgptˉ和lpgvtˉ_{lp}^{gv}\bar{t}lpgvtˉ，然后减去质心，得到中心化后的轨迹
Δlpgpti=lpgpti−lpgptˉΔlpgvtj=lpgvtj−lpgvtˉ\begin{aligned} \Delta_{lp}^{gp}t_i&=\ _{lp}^{gp}t_i -\ _{lp}^{gp}\bar{t} \\ \Delta_{lp}^{gv}t_j&=\ _{lp}^{gv}t_j -\ _{lp}^{gv}\bar{t} \end{aligned} ΔlpgptiΔlpgvtj= lpgpti− lpgptˉ= lpgvtj− lpgvtˉ

之后计算两条中心化轨迹中所有translation的协方差矩阵，即
S=1n∑inΔlpgpti∗Δlpgvtj(i)TS=\frac{1}{n}\sum_i^n\Delta_{lp}^{gp}t_i\ *\ \Delta_{lp}^{gv}t_{j(i)}^T S=n1i∑nΔlpgpti ∗ Δlpgvtj(i)T
对协方差矩阵做奇异值分解：
S=UΣVTS=U\Sigma V^T S=UΣVT
则两条轨迹之间的旋转和位移分别为（证明见参考文献[1]）：
gpgvR=VUTgpgvt=lpgvtˉ−gpgvR∗lpgptˉ\begin{aligned} _{gp}^{gv}R &= VU^T \\ _{gp}^{gv}t &=\ _{lp}^{gv}\bar{t} - ~_{gp}^{gv}R\ *\ _{lp}^{gp}\bar{t} \end{aligned} gpgvRgpgvt=VUT= lpgvtˉ− gpgvR ∗ lpgptˉ
若det(VUT)=−1det(VU^T)=-1det(VUT)=−1，将VVV的最后一列乘以−1-1−1后再用上式计算gpgvR_{gp}^{gv}RgpgvR。

求解lplvT_{lp}^{lv}TlplvT

通过上面的方法求出gpgvT_{gp}^{gv}TgpgvT，将其带回等式，并作为已知量固定，再来单独求解lplvT_{lp}^{lv}TlplvT。lplvT_{lp}^{lv}TlplvT是右乘量，不能描述为刚体变换。这里提供三种求解方法。

第一种方法，使用数值优化，目标函数与约束如下：
min⁡lplvT∑i∥gpgvT∗lpgpTi−lvgvTj(i)∗lplvT∥F2s.t.∥lplvq∥=1\begin{aligned} &\min_{_{lp}^{lv}T} \sum_{i}\Vert\ _{gp}^{gv}T\ *\ _{lp}^{gp}T_i\ -\ _{lv}^{gv}T_{j(i)}\ *\ _{lp}^{lv}T\ \Vert_F^2 \\ &s.t.\ \ \ \ \Vert\ _{lp}^{lv}q\ \Vert = 1 \end{aligned} lplvTmini∑∥ gpgvT ∗ lpgpTi − lvgvTj(i) ∗ lplvT ∥F2s.t. ∥ lplvq ∥=1
其中，lplvT_{lp}^{lv}TlplvT中的旋转量使用四元数表示，并要求四元数的模长为1。将约束项作为惩罚项合并到目标函数中，可以得到无约束的非线性优化问题：
min⁡lplvT∑i∥gpgvT∗lpgpTi−lvgvTj(i)∗lplvT∥F2+w(∥lplvq∥−1)2\min_{_{lp}^{lv}T} \sum_{i}\Vert\ _{gp}^{gv}T\ *\ _{lp}^{gp}T_i\ -\ _{lv}^{gv}T_{j(i)}\ *\ _{lp}^{lv}T\ \Vert_F^2 + w( \Vert\ _{lp}^{lv}q\ \Vert - 1)^2 lplvTmini∑∥ gpgvT ∗ lpgpTi − lvgvTj(i) ∗ lplvT ∥F2+w(∥ lplvq ∥−1)2
我们目前将www设为了轨迹中帧的数目。

第二种方法，可以推导出一种线性解法。设已知量gpgvT∗lpgpTi=lpgvTi_{gp}^{gv}T\ *\ _{lp}^{gp}T_i\ =\ _{lp}^{gv}T_igpgvT ∗ lpgpTi = lpgvTi，并将所有的变换矩阵TTT表示为旋转矩阵RRR和位移矢量ttt，由
lpgvTi=lvgvTj(i)∗lplvT_{lp}^{gv}T_i\ =\ _{lv}^{gv}T_{j(i)}\ *\ _{lp}^{lv}T lpgvTi = lvgvTj(i) ∗ lplvT
可得
lplvR=lvgvRj(i)T∗lpgvRilplvt=lvgvRj(i)T∗(lpgvti−lvgvtj(i))\begin{aligned} _{lp}^{lv}R\ &=\ _{lv}^{gv}R^T_{j(i)}\ *\ _{lp}^{gv}R_i \\ _{lp}^{lv}t\ &=\ _{lv}^{gv}R^T_{j(i)}\ *\ (\ _{lp}^{gv}t_i\ -\ _{lv}^{gv}t_{j(i)}\ ) \end{aligned} lplvR lplvt = lvgvRj(i)T ∗ lpgvRi= lvgvRj(i)T ∗ ( lpgvti − lvgvtj(i) )
其中，每一对lpgvTi_{lp}^{gv}T_ilpgvTi和lvgvTj(i)_{lv}^{gv}T_{j(i)}lvgvTj(i)均能求出一组lplvR_{lp}^{lv}RlplvR和lplvt_{lp}^{lv}tlplvt。最终的位移矢量lplvt_{lp}^{lv}tlplvt只需对所有通过上式求得的矢量取平均即可。但是，旋转量由于分布在流形上，要用特殊的方式进行平均。

首先设上式求得的每一个旋转量为lplvRi_{lp}^{lv}R_ilplvRi，将他们全部转换为四元数lplvqi_{lp}^{lv}q_ilplvqi，将四元数视为4x1的矢量，构成如下4xN矩阵：
Q=[lplvq1,lplvq2,...,lplvqn]Q=[\ _{lp}^{lv}q_1,\ _{lp}^{lv}q_2,\ ...,\ _{lp}^{lv}q_n\ ] Q=[ lplvq1, lplvq2, ..., lplvqn ]
然后对4x4矩阵Q∗QTQ*Q^TQ∗QT求特征值和特征向量，平均后的旋转量lplvq_{lp}^{lv}qlplvq即为最大特征值对应的特征向量（证明见参考文献[2]）

第三种方法，将其也转换为刚体变换问题。设已知量gpgvT∗lpgpTi=lpgvTi_{gp}^{gv}T\ *\ _{lp}^{gp}T_i\ =\ _{lp}^{gv}T_igpgvT ∗ lpgpTi = lpgvTi，则将原问题中的变换矩阵都求逆后，可得到如下优化问题：
min⁡lvlpT∑i∥gvlpTi−lvlpT∗gvlvTj(i)∥F2\min_{_{lv}^{lp}T} \sum_{i}\Vert\ _{gv}^{lp}T_i\ -\ _{lv}^{lp}T * \ _{gv}^{lv}T_{j(i)}\ \Vert_F^2 lvlpTmini∑∥ gvlpTi − lvlpT∗ gvlvTj(i) ∥F2
然后使用求解gpgvT_{gp}^{gv}TgpgvT的方法求出lvlpT_{lv}^{lp}TlvlpT，最终有lplvT=lvlpT−1_{lp}^{lv}T~=~_{lv}^{lp}T^{-1}lplvT = lvlpT−1。

三种方法在相同的输入与停止条件下，得到的结果基本上是一致的，只存在较小差异。某些输入下，某种方法要略好于其他，而另一种输入下，又可能略逊与其他。

交替迭代优化

求解出lplvT_{lp}^{lv}TlplvT后，又可以带回原等式，作为已知量，然后再次求解gpgvT_{gp}^{gv}TgpgvT。即不断进行上文的两个求解过程，交替优化gpgvT_{gp}^{gv}TgpgvT和lplvT_{lp}^{lv}TlplvT，直到两个轨迹的绝对误差不再缩小或小于某个阈值。

这个迭代的开始需要给lplvT_{lp}^{lv}TlplvT一个初值，由于VICON刚体与手机Body之间的位置差异不大，因此以单位变换作为初值即可。

时间戳对齐

以上算法求解的关键在于找到两个轨迹的对应帧lpgvTi_{lp}^{gv}T_ilpgvTi和lvgvTj(i)_{lv}^{gv}T_{j(i)}lvgvTj(i)，如果两个设备采集时的时间戳是完全同步的，即j(i)=ij(i)=ij(i)=i，则直接根据时间戳找对应关系即可。但是，实际上大多数设备在时间同步后，任然会有不同长度的时间差，少则几毫秒，多则几秒。为了解决这个问题，在执行上文算法前需要对两个轨迹数据估计时间差，并对齐时间戳。

时间戳和轨迹是离散记录的，无法直接使用优化方法得出时间差。常用方法为将轨迹通过样条插值变为连续曲线，这样就得到了时间和位姿的近似连续量，然后使用非线性优化的方式求时间差，详细可见参考文献[3]。

我们使用了一种搜索算法，实现更简单，计算效率更高，但精度稍低（毫秒级精度）。

首先假设两个轨迹的时间差在±10\pm 10±10秒以内，以某条轨迹作为reference（一般选帧率较高的一个，比如VICON），改变另外一条轨迹的时间戳，减10秒，减9秒，直到加9秒，加10秒，步长为1秒。这样就得到20条修改时间戳后的轨迹，每一条轨迹都和reference通过时间戳找对应帧（时间戳相差最小的帧），然后使用求解gpgvT_{gp}^{gv}TgpgvT的算法拟合两条轨迹，并计算绝对误差（只考虑帧之间的位置，不考虑姿态），取误差最小的轨迹对应的时间差，比如说−5-5−5秒。然后在−5±0.1-5\pm 0.1−5±0.1秒的区间内以0.10.10.1秒的步长再次做上述操作。直到以0.0010.0010.001秒作为步长搜索后结束。本质上是一种分层次搜索算法。为了提高搜索精度，还可以在使绝对误差最小的时间差附近拟合一条二次曲线（自变量是时间差，因变量是绝对误差），取二次曲线的最小值处的时间差作为最终结果。

实验结果

使用一条VICON轨迹（true_trajectory）人为添加外参变换和刚体变换后得到另外一条轨迹（fake_trajectory），同时也人为的加入噪声（0.1度的角度随机偏差，1cm的位置随机偏差）、时间差（与原轨迹相差5.421秒），并降采样（帧率为原轨迹的四分之一）。

拟合前

拟合后

两条轨迹高度重合，放大后可以看清彼此

参考文献

Least-Squares Rigid Motion Using SVD
Quaternion Averaging
Unified Temporal and Spatial Calibration for Multi-Sensor Systems