位姿表示方法
5、四元数的运算
5.1 乘法
def multiply_quaternions(q, r):
""" Takes in 2 1*4 quaternions, returns a 1*4 quaternion"""
"""Multiplies two quaternions 'q' and 'r'. Returns 't' where t = q*r"""
t = np.empty(([1,4]))
t[:,0] = r[:,0]*q[:,0] - r[:,1]*q[:,1] - r[:,2]*q[:,2] - r[:,3]*q[:,3]
t[:,1] = (r[:,0]*q[:,1] + r[:,1]*q[:,0] - r[:,2]*q[:,3] + r[:,3]*q[:,2])
t[:,2] = (r[:,0]*q[:,2] + r[:,1]*q[:,3] + r[:,2]*q[:,0] - r[:,3]*q[:,1])
t[:,3] = (r[:,0]*q[:,3] - r[:,1]*q[:,2] + r[:,2]*q[:,1] + r[:,3]*q[:,0])
return t- 共需要16次乘法和9次加法
5.2 除法
def divide_quaternions(q, r):
"""Divides two quaternions 'q' and 'r'. Returns quaternion t where t = q/r"""
t = np.empty([4, 1])
t[0] = ((r[0] * q[0]) + (r[1] * q[1]) + (r[2] * q[2]) + (r[3] * q[3])) / ((r[0] ** 2) + (r[1] ** 2) + (r[2] ** 2) + (r[3] ** 2))
t[1] = (r[0] * q[1] - (r[1] * q[0]) - (r[2] * q[3]) + (r[3] * q[2])) / (r[0] ** 2 + r[1] ** 2 + r[2] ** 2 + r[3] ** 2)
t[2] = (r[0] * q[2] + r[1] * q[3] - (r[2] * q[0]) - (r[3] * q[1])) / (r[0] ** 2 + r[1] ** 2 + r[2] ** 2 + r[3] ** 2)
t[3] = (r[0] * q[3] - (r[1] * q[2]) + r[2] * q[1] - (r[3] * q[0])) / (r[0] ** 2 + r[1] ** 2 + r[2] ** 2 + r[3] ** 2)
return t5.3 求逆
def inverse_quaternion(q):
"""Takes in a 1*4 quaternion. Returns a 1*4 quaternion. Returns the inverse of quaternion 'q'. Denoted by q^-1"""
t = np.empty([4, 1])
t[0] = q[:,0] / np.power(norm_quaternion(q),2)
t[1] = -q[:,1] / np.power(norm_quaternion(q),2)
t[2] = -q[:,2] / np.power(norm_quaternion(q),2)
t[3] = -q[:,3] / np.power(norm_quaternion(q),2)
t = np.transpose(t)
return t6、各种表示之间的转换
四元数转化为旋转矩阵
def quat2rot(q):
"""Converts a quaternion into a rotation matrix"""
# Using the second method listed on this link:
# http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
q = normalize_quaternion(q)
qhat = np.zeros([3,3])
qhat[0,1] = -q[:,3]
qhat[0,2] = q[:,2]
qhat[1,2] = -q[:,1]
qhat[1,0] = q[:,3]
qhat[2,0] = -q[:,2]
qhat[2,1] = q[:,1]
R = np.identity(3) + 2*np.dot(qhat,qhat) + 2*np.array(q[:,0])*qhat
#R = np.round(R,4)
return R旋转矩阵转化为欧拉角
def rot2euler(R):
""" Gets the euler angles corresponding to the rotation matrix R"""
phi = -math.asin(R[1,2])
theta = -math.atan2(-R[0,2]/math.cos(phi),R[2,2]/math.cos(phi))
psi = -math.atan2(-R[1,0]/math.cos(phi),R[1,1]/math.cos(phi))
return phi, theta, psi旋转矩阵转化为四元数
def rot2quat(R):
""" Converts from rotation matrix R into a quaternion"""
tr = R[0,0] + R[1,1] + R[2,2];
if tr > 0:
S = np.sqrt(tr+1.0) * 2
qw = 0.25 * S
qx = (R[2,1] - R[1,2]) / S
qy = (R[0,2] - R[2,0]) / S
qz = (R[1,0] - R[0,1]) / S
elif ((R[0,0] > R[1,1]) and (R[0,0] > R[2,2])):
S = np.sqrt(1.0 + R[0,0] - R[1,1] - R[2,2]) * 2
qw = (R[2,1] - R[1,2]) / S
qx = 0.25 * S
qy = (R[0,1] + R[1,0]) / S
qz = (R[0,2] + R[2,0]) / S
elif (R[1,1] > R[2,2]):
S = np.sqrt(1.0 + R[1,1] - R[0,0] - R[2,2]) * 2
qw = (R[0,2] - R[2,0]) / S
qx = (R[0,1] + R[1,0]) / S
qy = 0.25 * S
qz = (R[1,2] + R[2,1]) / S
else:
S = np.sqrt(1.0 + R[2,2] - R[0,0] - R[1,1]) * 2
qw = (R[1,0] - R[0,1]) / S
qx = (R[0,2] + R[2,0]) / S
qy = (R[1,2] + R[2,1]) / S
qz = 0.25 * S
q = [[qw],[qx],[qy],[qz]]
temp = np.sign(qw)
q = np.multiply(q,temp)
return q参考链接：https://github.com/bharath-r/orientation-tracking-unscented-kalman-filter/blob/master/BharathRamling_P2/quaternion_functions.py