在这个例子中,我们有两个四元数q1和q2,分别表示初始姿势和最终姿势。我们首先使用quat_diff函数计算这两个四元数之间的差。然后,我们使用quat_to_rot函数将得到的四元数差转换为旋转矩阵。最后,我们通过连接旋转矩阵和零平移向量,并在底部添加一行[0,0,0,1]来构造齐次变换矩阵T。
所得到的变换矩阵T可以用于将点或向量从初始姿态变换到最终姿态。
import numpy as np
# import g2o
def quat_mult(q1, q2):
"""Quaternion multiplication."""
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
return np.array([w, x, y, z])
def quat_inv(q):
"""Quaternion inverse."""
w, x, y, z = q
return np.array([w, -x, -y, -z])
def quat_to_rot(q):
"""Quaternion to rotation matrix."""
w, x, y, z = q
R = np.arra