旋转矩阵计算角度差

根据旋转矩阵的估计值和真值计算角度误差

论文 Benchmarking 6DOF Outdoor Visual Localization in Changing Conditions中公式

$$R_{gt}$$真值 $$R_{est}$$估计值 二维矩阵形式 $$2 \cos (|\alpha|)=\operatorname{trace}\left(\mathrm{R}_{\mathrm{gt}}^{-1} \mathrm{R}_{\mathrm{est}}\right)-1$$

def rot_error(r_gt,r_est):
    dis = abs(math.acos((np.trace(np.dot(np.linalg.inv(r_gt),r_est))-1)/2)) 
    #公式计算结果单位为弧度，转成角度返回
    return dis*180/math.pi

---

3维旋转矩阵计算角度差得到角度差矩阵

r_gt_matrix,r_est_matrix 3维矩阵

def rot_error_matrix(r_gt_matrix,r_est_matrix):
  return (np.abs(np.arccos(np.clip((np.trace(np.einsum("ahw,bjk->abhk", np.linalg.inv(r_gt_matrix),r_est_matrix),axis1=2,axis2=3)-1)/2,-1,1)))*180/math.pi)

E.G.

import numpy as np
import math
from einops import rearrange, reduce, repeat

r_gt=np.array([[1,0,0],[0,0,-1],[0,-1,0]])
r_est=np.array([[1,0,0],[0,0,1],[0,1,0]])

r_gt_matrix = repeat(r_gt,'i j -> c i j',c=4)
r_est_matrix = repeat(r_est,'i j -> c i j',c=5)

>>> rot_error(r_gt,r_est)
>>> 180.0

>>> rot_error_matrix((r_gt_matrix,r_est_matrix))
>>> array([[180., 180., 180., 180., 180.],
       		 [180., 180., 180., 180., 180.],
       		 [180., 180., 180., 180., 180.],
       		 [180., 180., 180., 180., 180.]])

参考

根据旋转矩阵计算角度差