由于旋转矩阵是正交矩阵,所以它的转置和逆矩阵相同
误差分为两个部分分别计算:
1.平移误差
E_{t} = \left \| t^{t}-t^{e} \right \|
2.旋转误差
下面是两种旋转误差的计算,算出来的结果不一样,我也不知道哪个是错的,或者说什么时候用哪一个:
2.1
E_{R}=arccos\frac{tr(R^{t}(R^{e})^{T})-1}{2}
其中t^{t}和t^{e}分别代表地面真实的平移和估计的平移;R^{t}和R^{e}分别代表地面真实的旋转和估计的旋转;tr()代表矩阵的轨迹。
PS:上面旋转误差计算的单位为弧度,若想转换为角度,则需要多乘以一个\frac{180}{pi}
即:E_{R}=arccos\frac{tr(R^{t}(R^{e})^{T})-1}{2}\times \frac{180}{pi}
引用论文:Benchmarking 6DOF Outdoor Visual Localization in Changing Conditions
2.2
matlab代码实现:
R_err = abs(acos((trace(inv(R_truth)*T_t(1:3,1:3))-1)/2))