计算旋转矩阵以在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的结果如下。我试图计算旋转矩阵以将绿色矢量与蓝色对齐。

Answer 1

问题出在这里：

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

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

我还会实施该答案评论中提到的附带案例（尤其是s=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))

Answer 3

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

Answer 4

基于@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