Rf = np.array([[-0.032, -0.999, 0.02 , 0.88 ],
[-0.999, 0.031, -0.008, -0.076],
[ 0.007, -0.02 , -1. , 0.409],
[ 0. , 0. , 0. , 0. ]])
theta = np.arccos((Rf[0,0]+Rf[1,1]+Rf[2,2]-1)/2)
kx = (Rf[2,1]-Rf[1,2])/2/np.sin(theta)
ky = (Rf[0,2]-Rf[2,0])/2/np.sin(theta)
kz = (Rf[1,0]-Rf[0,1])/2/np.sin(theta)
kxf = kx*theta
kyf = ky*theta
kzf = kz*theta
[Rf[0,3], Rf[1,3], Rf[2,3], kxf, kyf, kzf]
然而,(Rf [0,0] + Rf [1,1] + Rf [2,2] -1)/ 2 = -1,
所以theta是-180度。
np.sin(-180)等于0.
kx,yy,kz未定义。
如何处理此案件?并找到正确的角轴转换?感谢。