Since the rotation matrix is an orthogonal matrix, its transpose and inverse are the same
The error is divided into two parts and calculated separately:
1. Translation error
E_{t} = \left \| t^{t}-t^{e} \right \|
2. Rotation error
The following are the calculations of the two rotation errors. The calculated results are different. I don’t know which one is wrong, or when to use which one:
2.1
E_{R}=arccos\frac{tr(R^{t}(R^{e})^{T})-1}{2}
Where t^{t} and t^{e} represent the ground truth translation and estimated translation, respectively; R^{t} and R^{e} represent the ground truth rotation and estimated rotation, respectively; tr() represents the matrix traces of.
PS: The above rotation error is calculated in radians. If you want to convert it into an angle, you need to multiply it by one \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 code implementation:
R_err = abs(acos((trace(inv(R_truth)*T_t(1:3,1:3))-1)/2))