旋转矩阵计算角度差

旋转矩阵计算角度差

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

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

$R_{gt}$真值 $R_{est}$估计值 二维矩阵形式

$$2\cos (|\alpha |)=\mathrm{trace}\left({\mathrm{R}}_{\mathrm{g}\mathrm{t}}^{-1}{\mathrm{R}}_{\mathrm{e}\mathrm{s}\mathrm{t}}\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.]])

参考

根据旋转矩阵计算角度差