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

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

问题描述

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

数学关系

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

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

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

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

这几个变换满足：
$${}_{gp}^{gv}T\ast {}_{lp}^{gp}T={}_{lv}^{gv}T\ast {}_{lp}^{lv}T$$

其中${}_{lp}^{gp}T$和${}_{lv}^{gv}T$均是已知量，多个${}_{lp}^{gp}T$和${}_{lv}^{gv}T$各自构成了两条运动轨迹，轨迹中的每一帧记为${}_{lp}^{gp}{T}_i$和${}_{lv}^{gv}{T}_j$。${}_{gp}^{gv}T$和${}_{lp}^{lv}T$是待求解的未知量。如果手机与VICON刚体是刚性连接的，在整个运动过程中，${}_{gp}^{gv}T$和${}_{lp}^{lv}T$的值是定值。

求解方法

优化的目标函数为：
$$\underset{{}_{gp}^{gv}T,{}_{lp}^{lv}T}{\min }\sum _i\Vert {}_{gp}^{gv}T\ast {}_{lp}^{gp}{T}_i-{}_{lv}^{gv}{T}_{j(i)}\ast {}_{lp}^{lv}T{\Vert }_F^2$$
其中${}_{lv}^{gv}{T}_{j(i)}$表示在时间戳上与${}_{lp}^{gp}{T}_i$对应的某一帧。

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

1. 自由度过大，如果旋转用四元数表示，上式要同时优化12个自由量。
2. 约束不足，存在奇异解，比如 g p g v T _{gp}^{gv}T