已知
经SLAM系统估计得到的相机位姿为
P
1
,
.
.
.
,
P
n
∈
S
E
(
3
)
\boldsymbol{P}_1,...,\boldsymbol{P}_n\in{SE(3)}
P1,...,Pn∈SE(3);
相机位姿真实值(ground truth)为
Q
1
,
.
.
.
,
Q
n
∈
S
E
(
3
)
\boldsymbol{Q}_1,...,\boldsymbol{Q}_n\in{SE(3)}
Q1,...,Qn∈SE(3).
相对位姿误差Relative pose error (RPE)
相对位姿误差通常用于评估视觉里程计。
其定义为
E
i
:
=
(
Q
i
−
1
Q
i
+
Δ
)
−
1
(
P
i
−
1
P
i
+
Δ
)
\boldsymbol{E}_i:=(\boldsymbol{Q}_i^{-1}\boldsymbol{Q}_{i+\Delta})^{-1}(\boldsymbol{P}_i^{-1}\boldsymbol{P}_{i+\Delta})
Ei:=(Qi−1Qi+Δ)−1(Pi−1Pi+Δ)
其中
Δ
\Delta
Δ是时间间隔。对于存在n个位姿的序列来说,可以获得
m
=
n
−
Δ
m=n-\Delta
m=n−Δ个相对位姿误差。
对于m个相对位姿误差,计算其误差位姿平移部分的均方根误差(root mean squared error (RMSE)):
R
M
S
E
(
E
1
:
n
,
Δ
)
:
=
(
1
m
∑
i
=
1
m
∥
t
r
a
n
s
(
E
i
)
∥
2
)
1
2
RMSE(\boldsymbol{E}_{1:n},\Delta):=(\frac{1}{m}\sum_{i=1}^m\parallel trans(\boldsymbol{E}_i)\parallel^2)^{\frac{1}{2}}
RMSE(E1:n,Δ):=(m1i=1∑m∥trans(Ei)∥2)21
其中
Δ
∈
1
:
n
\Delta\in{1:n}
Δ∈1:n按需求取值。
也可以计算
Δ
\Delta
Δ所有可能取值的结果平均值:
R
M
S
E
(
E
1
:
n
)
:
=
1
n
∑
Δ
=
1
n
R
M
S
E
(
E
1
:
n
,
Δ
)
RMSE(\boldsymbol{E}_{1:n}):=\frac{1}{n}\sum_{\Delta=1}^nRMSE(\boldsymbol{E}_{1:n},\Delta)
RMSE(E1:n):=n1Δ=1∑nRMSE(E1:n,Δ)
绝对轨迹误差Absolute trajectory error (ATE)
绝对轨迹误差通常用于评估整个slam系统。
首先对齐真实轨迹和估计轨迹,得到两者轨迹之间的最小二乘解变换矩阵
S
\boldsymbol{S}
S,则绝对轨迹误差定义如下:
F
i
:
=
Q
i
−
1
S
P
i
\boldsymbol{F}_i:=\boldsymbol{Q}_i^{-1}\boldsymbol{S}\boldsymbol{P}_i
Fi:=Qi−1SPi
同样我们计算所有时刻位姿误差平移部分的均方根误差(RMSE):
R
M
S
E
(
F
1
:
n
)
:
=
(
1
n
∑
i
=
1
n
∥
t
r
a
n
s
(
F
i
)
∥
2
)
1
2
RMSE(\boldsymbol{F}_{1:n}):=(\frac{1}{n}\sum_{i=1}^n\parallel trans(\boldsymbol{F}_i)\parallel^2)^{\frac{1}{2}}
RMSE(F1:n):=(n1i=1∑n∥trans(Fi)∥2)21