位姿表示方法

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 t

5.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 t

6、各种表示之间的转换

四元数转化为旋转矩阵

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

7、四元数的求导

 

参考链接：https://github.com/bharath-r/orientation-tracking-unscented-kalman-filter/blob/master/BharathRamling_P2/quaternion_functions.py