问题描述
SLAM中常常碰到对齐（align）两个不同设备采集的轨迹的问题。比如通过VICON跟踪获得了一组轨迹，手机通过SLAM算法也获得一组轨迹，要评估SLAM算法的精度，就需要将手机获得的轨迹与作为真值的VICON轨迹对齐。
数学关系
设代表手机（phone）从当前局部坐标系（local）到SLAM算法定义的世界坐标系（global）的变换，即当前手机在SLAM算法定义的世界坐标系下的位姿。
设代表从VICON的当前刚体坐标系到VICON定义的世界坐标系的变换，即VICON跟踪的刚体在VICON世界坐标系下的位姿。
设代表两个世界坐标系之间的变换
设代表当前刚体坐标系与手机坐标系之间的变换
这几个变换满足：
其中和均是已知量，多个和各自构成了两条运动轨迹，轨迹中的每一帧记为和。和是待求解的未知量。如果手机与VICON刚体是刚性连接的，在整个运动过程中，和的值是定值。
求解方法
优化的目标函数为：
其中表示在时间戳上与对应的某一帧。
上式不好直接优化，原因在于
- 自由度过大，如果旋转用四元数表示，上式要同时优化12个自由量。
- 约束不足，存在奇异解，比如和均为零，显然是上式的一个极小解。
因此考虑分部优化。
求解
首先假定已知，只求解，相当于是寻找一个刚体变换将已知轨迹变换为另一个已知轨迹，并满足最小二乘。这类问题有一个高效的线性解法。
设，两条轨迹的translation部分分别为和，将轨迹中的所有translation求均值得到质心和，然后减去质心，得到中心化后的轨迹
之后计算两条中心化轨迹中所有translation的协方差矩阵，即
对协方差矩阵做奇异值分解：
则两条轨迹之间的旋转和位移分别为（证明见参考文献[1]）：
若，将的最后一列乘以后再用上式计算。
求解
通过上面的方法求出，将其带回等式，并作为已知量固定，再来单独求解。是右乘量，不能描述为刚体变换。这里提供三种求解方法。
第一种方法，使用数值优化，目标函数与约束如下：
其中，中的旋转量使用四元数表示，并要求四元数的模长为1。 将约束项作为惩罚项合并到目标函数中，可以得到无约束的非线性优化问题：
我们目前将设为了轨迹中帧的数目。
第二种方法，可以推导出一种线性解法。设已知量，并将所有的变换矩阵表示为旋转矩阵和位移矢量，由
可得
其中，每一对和均能求出一组和。最终的位移矢量只需对所有通过上式求得的矢量取平均即可。但是，旋转量由于分布在流形上，要用特殊的方式进行平均。
首先设上式求得的每一个旋转量为，将他们全部转换为四元数，将四元数视为4x1的矢量，构成如下4xN矩阵：
然后对4x4矩阵求特征值和特征向量，平均后的旋转量即为最大特征值对应的特征向量（证明见参考文献[2]）
第三种方法，将其也转换为刚体变换问题。设已知量，则将原问题中的变换矩阵都求逆后，可得到如下优化问题：
然后使用求解的方法求出，最终有。
三种方法在相同的输入与停止条件下，得到的结果基本上是一致的，只存在较小差异。某些输入下，某种方法要略好于其他，而另一种输入下，又可能略逊与其他。
交替迭代优化
求解出后，又可以带回原等式，作为已知量，然后再次求解。即不断进行上文的两个求解过程，交替优化和，直到两个轨迹的绝对误差不再缩小或小于某个阈值。
这个迭代的开始需要给一个初值，由于VICON刚体与手机Body之间的位置差异不大，因此以单位变换作为初值即可。
时间戳对齐
以上算法求解的关键在于找到两个轨迹的对应帧和，如果两个设备采集时的时间戳是完全同步的，即，则直接根据时间戳找对应关系即可。但是，实际上大多数设备在时间同步后，任然会有不同长度的时间差，少则几毫秒，多则几秒。为了解决这个问题，在执行上文算法前需要对两个轨迹数据估计时间差，并对齐时间戳。
时间戳和轨迹是离散记录的，无法直接使用优化方法得出时间差。常用方法为将轨迹通过样条插值变为连续曲线，这样就得到了时间和位姿的近似连续量，然后使用非线性优化的方式求时间差，详细可见参考文献[3]。
我们使用了一种搜索算法，实现更简单，计算效率更高，但精度稍低（毫秒级精度）。
首先假设两个轨迹的时间差在秒以内，以某条轨迹作为reference（一般选帧率较高的一个，比如VICON），改变另外一条轨迹的时间戳，减10秒，减9秒，直到加9秒，加10秒，步长为1秒。这样就得到20条修改时间戳后的轨迹，每一条轨迹都和reference通过时间戳找对应帧（时间戳相差最小的帧），然后使用求解的算法拟合两条轨迹，并计算绝对误差（只考虑帧之间的位置，不考虑姿态），取误差最小的轨迹对应的时间差，比如说秒。然后在秒的区间内以秒的步长再次做上述操作。直到以秒作为步长搜索后结束。本质上是一种分层次搜索算法。为了提高搜索精度，还可以在使绝对误差最小的时间差附近拟合一条二次曲线（自变量是时间差，因变量是绝对误差），取二次曲线的最小值处的时间差作为最终结果。
时间搜索的精度依赖于两条轨迹的拟合精度，手机与VICON刚体的轨迹之间存在外参，要使轨迹拟合良好，需要将VICON刚体的轨迹通过该外参变换为手机轨迹。考虑到时间差、以及之间的耦合关系，我们仍然采用交替优化的思想，得到如下优化步骤：
- 初始化为单位变换；
- 用变换VICON轨迹后，搜索时间差并对齐时间戳；
- 求解轨迹之间的和，少量迭代即可；
- 多次重复2到3步；
- 求解轨迹之间的和，直到误差收敛。
实验结果
使用一条VICON轨迹（true_trajectory）人为添加外参变换和刚体变换后得到另外一条轨迹（fake_trajectory），同时也人为的加入噪声（0.1度的角度随机偏差，1cm的位置随机偏差）、时间差（与原轨迹相差5.421秒），并降采样（帧率为原轨迹的四分之一）。
拟合前
拟合后
两条轨迹高度重合，放大后可以看清彼此
