计算旋转矩阵以对齐 3D 空间中的两个向量？

Question

我有两个独立的 3D 数据点向量，它们表示曲线，我正在使用 matplotlib 将它们绘制为 3D 图中的散点数据。

两个向量都从原点开始，并且都是单位长度。曲线彼此相似，但是，两条曲线之间通常存在旋转（出于测试目的，我实际上使用一条曲线并对其应用旋转矩阵以创建第二条曲线）。

我想对齐两条曲线，使它们在 3D 中对齐，例如旋转曲线 b，使其起点和终点与曲线 a 对齐。我一直在尝试通过从第一个点减去最后一个点来做到这一点，以获得表示从每条曲线的起点到终点的直线的方向矢量，将它们转换为单位矢量，然后计算交叉和点积和使用此答案 ( https://math.stackexchange.com/a/476311/357495 ) 中概述的方法来计算旋转矩阵。

但是，当我这样做时，计算出的旋转矩阵是错误的，我不确定为什么？

我的代码如下（我使用的是 Python 2.7）：

 # curve_1, curve_2 are arrays of 3D points, of the same length (both start at the origin)

curve_vec_1 = (curve_1[0] - curve_1[-1]).reshape(3,1)
curve_vec_2 = (curve_2[index][0] - curve_2[index][-1]).reshape(3,1)
a,b = (curve_vec_1/ np.linalg.norm(curve_vec_1)).reshape(3), (curve_vec_2/ np.linalg.norm(curve_vec_2)).reshape(3)
v = np.cross(a,b)
c = np.dot(a,b)
s = np.linalg.norm(v)
I = np.identity(3)
vXStr = '{} {} {}; {} {} {}; {} {} {}'.format(0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0)
k = np.matrix(vXStr)
r = I + k + np.square(k) * ((1 -c)/(s**2))

for i in xrange(item.shape[0]):
    item[i] = (np.dot(r, item[i]).reshape(3,1)).reshape(3)

在我的测试用例中，曲线 2 只是应用了以下旋转矩阵的曲线 1：

 [[1  0       0    ]
[ 0  0.5     0.866]
[ 0  -0.866  0.5  ]]

（仅绕 x 轴旋转 60 度）。

由我的代码计算以再次对齐两个向量的旋转矩阵是：

 [[ 1.         -0.32264329  0.27572962]
 [ 0.53984249  1.         -0.35320293]
 [-0.20753816  0.64292975  1.        ]]

两条原始曲线（分别为蓝色和绿色的 a 和 b）的方向向量图以及使用计算出的旋转矩阵（红色）转换的 b 的结果如下所示。我正在尝试计算旋转矩阵以将绿色向量与蓝色向量对齐。

原文由 Mark 发布，翻译遵循 CC BY-SA 4.0 许可协议

Answer 1

问题在这里：

 r = I + k + np.square(k) * ((1 -c)/(s**2))

np.square(k) 对矩阵的每个元素进行平方。您需要 np.matmul(k,k) 或 k @ k 矩阵乘以自身。

我还会实施该答案的评论中提到的附带案例（尤其是 s=0 ），否则您最终会在很多情况下出错。

原文由 Daniel F 发布，翻译遵循 CC BY-SA 3.0 许可协议

Answer 2

根据 Daniel F 的更正，这里有一个函数可以执行您想要的操作：

 import numpy as np

def rotation_matrix_from_vectors(vec1, vec2):
    """ Find the rotation matrix that aligns vec1 to vec2
    :param vec1: A 3d "source" vector
    :param vec2: A 3d "destination" vector
    :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
    """
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
    v = np.cross(a, b)
    c = np.dot(a, b)
    s = np.linalg.norm(v)
    kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
    rotation_matrix = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
    return rotation_matrix

测试：

 vec1 = [2, 3, 2.5]
vec2 = [-3, 1, -3.4]

mat = rotation_matrix_from_vectors(vec1, vec2)
vec1_rot = mat.dot(vec1)
assert np.allclose(vec1_rot/np.linalg.norm(vec1_rot), vec2/np.linalg.norm(vec2))

原文由 Peter 发布，翻译遵循 CC BY-SA 4.0 许可协议