SLAM系统评估公式

已知

经SLAM系统估计得到的相机位姿为${\mathbf{P}}_1,...,{\mathbf{P}}_n\in SE(3)$;
相机位姿真实值(ground truth)为${\mathbf{Q}}_1,...,{\mathbf{Q}}_n\in SE(3)$.

相对位姿误差Relative pose error (RPE)

相对位姿误差通常用于评估视觉里程计。
其定义为
$${\mathbf{E}}_i:=({\mathbf{Q}}_i^{-1}{\mathbf{Q}}_{i+\Delta }{)}^{-1}({\mathbf{P}}_i^{-1}{\mathbf{P}}_{i+\Delta })$$
其中$\Delta$是时间间隔。对于存在n个位姿的序列来说，可以获得$m=n-\Delta$个相对位姿误差。
对于m个相对位姿误差，计算其误差位姿平移部分的均方根误差(root mean squared error (RMSE))：
$$RMSE({\mathbf{E}}_{1:n},\Delta ):=(\frac{1}{m}\sum _{i=1}^m\parallel trans({\mathbf{E}}_i){\parallel }^2{)}^{\frac{1}{2}}$$
其中$\Delta \in 1:n$按需求取值。
也可以计算$\Delta$所有可能取值的结果平均值：
$$RMSE({\mathbf{E}}_{1:n}):=\frac{1}{n}\sum _{\Delta =1}^{n}RMSE({\mathbf{E}}_{1:n},\Delta )$$

绝对轨迹误差Absolute trajectory error (ATE)

绝对轨迹误差通常用于评估整个slam系统。
首先对齐真实轨迹和估计轨迹，得到两者轨迹之间的最小二乘解变换矩阵$\mathbf{S}$,则绝对轨迹误差定义如下：
$${\mathbf{F}}_i:={\mathbf{Q}}_i^{-1}\mathbf{S}{\mathbf{P}}_i$$
同样我们计算所有时刻位姿误差平移部分的均方根误差(RMSE)：
$$RMSE({\mathbf{F}}_{1:n}):=(\frac{1}{n}\sum _{i=1}^n\parallel trans({\mathbf{F}}_i){\parallel }^2{)}^{\frac{1}{2}}$$