旋转矩阵的误差计算

由于旋转矩阵是正交矩阵，所以它的转置和逆矩阵相同

误差分为两个部分分别计算：

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))