SLAM十四讲第三章第7题 Python Numpy实现 & 坑

题目是很直观的，只需把一号的位姿和二号的位姿分别成矩阵表示的T，也就是将四元数q转为旋转矩阵的形式，再拼上平移向量t，组成最后的T

但如果用python实现的话有不少坑：

首先要注意题目中说了将q归一化后再计算，归一化的意思是让向量的模长为1，即$||q||=1$，python的实现为

q1 = q1 / np.linalg.norm(q1)

要注意，《SLAM十四讲》中对于四元数定义的是$q(w,x,y,z)$，第一个w是实部，所以题目中q2, q2的第一个数0.35和-0.5也都是实部

所以如果直接使用四元数 → 旋转矩阵的计算公式来算那没有大问题

def quaternion_to_rotation_matrix(q):
    w, x, y, z = q
    return 2 * np.array([
        [0.5-y**2-z**2, x*y-z*w, x*z+y*w],
        [x*y+z*w, 0.5-x**2-z**2, y*z-x*w],
        [x*z-y*w, y*z+x*w, 0.5-x**2-y**2]
    ])

但如果你想用Python中的库，比如scipy中的Rotation来将四元数转为旋转矩阵，如下代码很简洁

from scipy.spatial.transform import Rotation as R

def quaternion2rot(quaternion):
    r = R.from_quat(quaternion)
    rot = r.as_matrix()
    return rot

但！！！或许从上面的公式截图中你已经看到了，在其他书籍、库中更常用的是$q(x,y,z,w)$这种形式，所以如果直接调用scipy的R.from_quat()会得到错误的结果，需要转换下参数的顺序再调用

q1_ = np.concatenate((q1[1:], q1[:1]))
q2_ = np.concatenate((q2[1:], q2[:1]))
R1, R2 = quaternion2rot(q1_), quaternion2rot(q2_)

该题最终的代码为

import numpy as np
import math
from scipy.spatial.transform import Rotation as R
 
 
def quaternion2rot(quaternion):
    r = R.from_quat(quaternion)
    rot = r.as_matrix()
    return rot

def quaternion_to_rotation_matrix(q):
    w, x, y, z = q
    return 2 * np.array([
        [0.5-y**2-z**2, x*y-z*w, x*z+y*w],
        [x*y+z*w, 0.5-x**2-z**2, y*z-x*w],
        [x*z-y*w, y*z+x*w, 0.5-x**2-y**2]
    ])

q1 = np.array([0.35, 0.2, 0.3, 0.1])
t1 = np.array([0.3, 0.1, 0.1]).T

q2 = np.array([-0.5, 0.4, -0.1, 0.2])
t2 = np.array([-0.1, 0.5, 0.3]).T

p = np.array([0.5, 0, 0.2]).T

q1 = q1 / np.linalg.norm(q1)
q2 = q2 / np.linalg.norm(q2)

# (1) 根据公式手写转换
# R1, R2 = quaternion_to_rotation_matrix(q1), quaternion_to_rotation_matrix(q2)

# (2) 调用scipy库
q1_ = np.concatenate((q1[1:], q1[:1]))
q2_ = np.concatenate((q2[1:], q2[:1]))
R1, R2 = quaternion2rot(q1_), quaternion2rot(q2_)

T1 = np.eye(4)
T1[0:3, 0:3] = R1
T1[0:3, -1] = t1

T2 = np.eye(4)
T2[0:3, 0:3] = R2
T2[0:3, -1] = t2
print("T1_c2w\n", T1)
print("T2_c2w\n", T2)

p_ = T2 @ np.linalg.inv(T1) @ np.hstack((p, 1))
print("p'\n", p_[0:3].T)

两个变换矩阵和最终结果为

T1_c2w
 [[ 0.23809524  0.19047619  0.95238095  0.3       ]
 [ 0.72380952  0.61904762 -0.3047619   0.1       ]
 [-0.64761905  0.76190476  0.00952381  0.1       ]
 [ 0.          0.          0.          1.        ]]
 
T2_c2w
 [[ 0.7826087   0.26086957  0.56521739 -0.1       ]
 [-0.60869565  0.13043478  0.7826087   0.5       ]
 [ 0.13043478 -0.95652174  0.26086957  0.3       ]
 [ 0.          0.          0.          1.        ]]
 
p'
 [-0.03097308  0.73498965  0.29610766]

写在最后，不懂为啥网上那么多人给出的答案都不一致，也没人简单深入研究下这事