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

Question

我有两个分离的三维数据点向量，表示曲线，并在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

---

你也可以围绕平均向量旋转180度，不需要使用叉积或内积。 - anishtain4

Answer 1

根据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

---

这不是右手定则，对吧？ [1,0,0] 和 [0,0,1] 绕 y 轴逆时针旋转 +90 度，得到 [[ 0., 0., -1.],[ 0., 1., 0.],[ 1., 0., 0.]]。 - Alexander Cska

@AlexanderCska 我认为这个约定并不重要，因为转换矩阵是一个向量到另一个向量的影响因素集合。所选答案确实对旋转矩阵的定义是正确的。 - Kevin R.

Answer 2

问题就在这里：

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

np.square(k)会将矩阵中的每个元素平方。你需要使用np.matmul(k,k)或者k @ k，这样可以让矩阵自己乘以自己。

我还建议您实现该答案评论中提到的边界情况（特别是s=0），否则在很多情况下会出现错误。

- Daniel F

---

谢谢，问题已解决！由于我使用的是numpy < 1.10版本，所以我使用了np.dot(k,k)而不是matmul。我将实现边缘情况，只是需要先让基本情况正常工作。 :) - Mark

Answer 3

基于 @Peter 和 @Daniel F 的工作。上述函数在我的情况下可行，除了在方向向量相同时 v 将为零向量的情况。我在此处捕获并返回标识向量。

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 / numpy.linalg.norm(vec1)).reshape(3), (vec2 / numpy.linalg.norm(vec2)).reshape(3)
    v = numpy.cross(a, b)
    if any(v): #if not all zeros then 
        c = numpy.dot(a, b)
        s = numpy.linalg.norm(v)
        kmat = numpy.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
        return numpy.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))

    else:
        return numpy.eye(3) #cross of all zeros only occurs on identical directions

- Kevin R.

Answer 4

可以使用scipy来实现这一点，使用scipy的Rotation函数重现@Peter的答案，请参见： https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html?highlight=scipy%20spatial%20transform%20rotation#scipy.spatial.transform.Rotation

from scipy.spatial.transform import Rotation as R
import numpy as np

def get_rotation_matrix(vec2, vec1=np.array([1, 0, 0])):
    """get rotation matrix between two vectors using scipy"""
    vec1 = np.reshape(vec1, (1, -1))
    vec2 = np.reshape(vec2, (1, -1))
    r = R.align_vectors(vec2, vec1)
    return r[0].as_matrix()


vec1 = np.array([2, 3, 2.5])
vec2 = np.array([-3, 1, -3.4])

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

祝好，马库斯

- Markus Kaukonen

---

谢谢您指出这个问题，但我不建议使用这个函数来对齐两个向量，这个函数是“估计旋转以最优地对齐两组向量”。当我尝试使用它时，有时会出现警告：“用户警告：给定的向量集的最优旋转不是唯一或定义不清”，并且对齐效果不好。 - XueYu

@XueYu，你能提供一个它失败的例子吗？ - Markus Kaukonen

我可能对齐方式有误，因为我从未真正看到断言行出错。您可以设置vec1 = np.random.rand(3)，vec2 = np.random.rand(3)，并运行几次，您将看到警告。例如：vec1 = np.array([0.08067752,0.86080858,0.19369599])，vec2 = np.array([0.7616154, 0.17332016, 0.09992052])。 - XueYu

是的，你说得对，你的数字有警告（但旋转后的单位向量与6位数字是正确的）。也许在检查后接受旋转矩阵是一个好主意。也许数学方面更有知识的人可以评论这个警告... - Markus Kaukonen

Answer 5

我认为如果您没有旋转轴，则旋转矩阵不是唯一的。

- weeshin

---

这听起来更像是一条评论而不是答案。 - Yan Sklyarenko

抱歉，由于声望不足，我无法发表评论 :( - weeshin