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

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

在视觉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=1N​​log(Tgt,i−1​Testi,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=1N​​trans(Tgt,i−1​Testi,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−1​Testi,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−Δt​​log((Tgt,i−1​Tgt,i+Δt​)−1(Testi,i−1​Testi,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−Δt​​trans((Tgt,i−1​Tgt,i+Δt​)−1(Testi,i−1​Testi,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−1​Tgt,i+Δt​计算的是真实轨迹在iii时刻到i+Δti + \Delta ti+Δt时刻的相对位姿变换。
  • Testi,i−1Testi,i+ΔtT_{esti,i}^{-1} T_{esti,i + \Delta t}Testi,i−1​Testi,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−1​Tgt,i+Δt​)−1(Testi,i−1​Testi,i+Δt​)计算的是估计的相对位姿变换相对于真实相对位姿变换的差异。
  • 其余符号含义与 ATE 公式中相同。
• 两种形式
  • RPEallRPE_{all}RPEall​考虑了相对位姿的旋转和平移误差，通过李代数的方式全面评估相对位姿变化的精度。
  • RPEtransRPE_{trans}RPEtrans​仅考虑相对平移误差，适用于重点关注相对平移精度的情况。

这些误差指标为评估视觉SLAM等算法的轨迹估计性能提供了量化的标准，帮助开发者了解算法在不同方面的精度表现，从而进行优化和改进。