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

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 将是零向量。我在这里抓住了这个，而不是 return 身份向量。

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

Answer 5

为此可以使用 scipy，在此处复制@Peter 用 scipy 旋转的答案，请参阅： 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))

特维辛，马库斯

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

Calculate rotation matrix to align two vectors in 3D space?

python

3d

numpy

python-2.7

rotational-matrices