当前位置：首页 > news >正文

视觉SLAM十四讲(v2)-(4.4.2)例子:详细解释评估轨迹的误差公式

news 2026/6/28 0:39:43

在视觉SLAM等实际工程中，评估算法估计轨迹与真实轨迹的差异至关重要，以下将详细解释绝对轨迹误差（ATE）和相对位姿误差（RPE）这两种评估轨迹误差的公式。

绝对轨迹误差（Absolute Trajectory Error, ATE）

公式

ATEall=1N∑i=1N∥log⁡(Tgt,i−1Testi,i)∨∥22ATE_{all} = \sqrt{\frac{1}{N} \sum_{i = 1}^{N} \left\| \log \left( T_{gt,i}^{-1} T_{esti,i} \right)^{\vee} \right\|_{2}^{2}}ATEall=N1∑i=1Nlog(Tgt,i−1Testi,i)∨22
ATEtrans=1N∑i=1N∥trans(Tgt,i−1Testi,i)∥22ATE_{trans} = \sqrt{\frac{1}{N} \sum_{i = 1}^{N} \left\| \text{trans} \left( T_{gt,i}^{-1} T_{esti,i} \right) \right\|_{2}^{2}}ATEtrans=N1∑i=1Ntrans(Tgt,i−1Testi,i)22

解释

整体含义：ATE用于衡量估计轨迹与真实轨迹之间的整体差异，反映了估计轨迹相对于真实轨迹的旋转和平移误差的综合情况。
符号说明
- NNN表示轨迹中的位姿数量，iii是位姿的索引，i=1,⋯ ,Ni = 1, \cdots, Ni=1,⋯,N。
- Tgt,iT_{gt,i}Tgt,i是真实轨迹在第iii个时刻的位姿，Testi,iT_{esti,i}Testi,i是估计轨迹在第iii个时刻的位姿。
- Tgt,i−1Testi,iT_{gt,i}^{-1} T_{esti,i}Tgt,i−1Testi,i计算的是估计位姿相对于真实位姿的变换关系。
- log⁡(⋅)∨\log(\cdot)^{\vee}log(⋅)∨操作是将李群空间中的变换映射到李代数空间。在三维空间中，李代数可以表示旋转和平移的微小变化，通过这个操作可以将位姿的差异转换为更容易处理的向量形式。
- ∥⋅∥2\left\| \cdot \right\|_{2}∥⋅∥2是L2L_2L2范数，用于计算向量的模长。
- trans(⋅)\text{trans}(\cdot)trans(⋅)函数用于提取变换矩阵中的平移部分。
两种形式
- ATEallATE_{all}ATEall考虑了位姿的旋转和平移误差，通过李代数的方式将旋转和平移统一表示并计算其均方根误差，能够全面反映轨迹的整体误差情况。
- ATEtransATE_{trans}ATEtrans仅考虑平移误差，通过提取变换矩阵中的平移部分并计算其均方根误差，适用于主要关注轨迹平移精度的情况。

相对位姿误差（Relative Pose Error, RPE）

公式

RPEall=1N−Δt∑i=1N−Δt∥log⁡((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∨∥22RPE_{all} = \sqrt{\frac{1}{N - \Delta t} \sum_{i = 1}^{N - \Delta t} \left\| \log \left( \left( T_{gt,i}^{-1} T_{gt,i + \Delta t} \right)^{-1} \left( T_{esti,i}^{-1} T_{esti,i + \Delta t} \right) \right)^{\vee} \right\|_{2}^{2}}RPEall=N−Δt1∑i=1N−Δtlog((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∨22
RPEtrans=1N−Δt∑i=1N−Δt∥trans((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))∥22RPE_{trans} = \sqrt{\frac{1}{N - \Delta t} \sum_{i = 1}^{N - \Delta t} \left\| \text{trans} \left( \left( T_{gt,i}^{-1} T_{gt,i + \Delta t} \right)^{-1} \left( T_{esti,i}^{-1} T_{esti,i + \Delta t} \right) \right) \right\|_{2}^{2}}RPEtrans=N−Δt1∑i=1N−Δttrans((Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt))22

解释

整体含义：RPE衡量的是在固定时间间隔Δt\Delta tΔt内，估计轨迹的相对位姿变化与真实轨迹的相对位姿变化之间的差异，主要用于评估算法在短时间内的运动估计精度。
符号说明
- Δt\Delta tΔt是固定的时间间隔，表示要比较的相邻位姿之间的时间差。
- Tgt,i−1Tgt,i+ΔtT_{gt,i}^{-1} T_{gt,i + \Delta t}Tgt,i−1Tgt,i+Δt计算的是真实轨迹在iii时刻到i+Δti + \Delta ti+Δt时刻的相对位姿变换。
- Testi,i−1Testi,i+ΔtT_{esti,i}^{-1} T_{esti,i + \Delta t}Testi,i−1Testi,i+Δt计算的是估计轨迹在iii时刻到i+Δti + \Delta ti+Δt时刻的相对位姿变换。
- (Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt)\left( T_{gt,i}^{-1} T_{gt,i + \Delta t} \right)^{-1} \left( T_{esti,i}^{-1} T_{esti,i + \Delta t} \right)(Tgt,i−1Tgt,i+Δt)−1(Testi,i−1Testi,i+Δt)计算的是估计的相对位姿变换相对于真实相对位姿变换的差异。
- 其余符号含义与 ATE 公式中相同。
两种形式
- RPEallRPE_{all}RPEall考虑了相对位姿的旋转和平移误差，通过李代数的方式全面评估相对位姿变化的精度。
- RPEtransRPE_{trans}RPEtrans仅考虑相对平移误差，适用于重点关注相对平移精度的情况。