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

【问题标题】：Calculate rotation matrix to align two vectors in 3D space?计算旋转矩阵以对齐 3D 空间中的两个向量？
【发布时间】：2017-12-21 21:58:19
【问题描述】：

我有两个独立的 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 用计算的旋转矩阵（红色）变换的结果如下。我正在尝试计算旋转矩阵以将绿色矢量与蓝色对齐。

【问题讨论】：

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

标签： python python-2.7 numpy 3d rotational-matrices

【解决方案1】：

问题就在这里：

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

np.square(k) 将矩阵的每个元素平方。你想要np.matmul(k,k) 或k @ k 这是矩阵自身相乘。

我还将实施该答案的 cmets 中提到的侧面案例（尤其是 s=0），否则您最终会在很多案例中出现错误。

【讨论】：

谢谢 - 这解决了它！当我使用 numpy

【解决方案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))

【讨论】：

这不是右手定则，不是吗？ [1,0,0] 和 [0,0,1]，即绕 y 轴旋转 +90 度（逆时针），轴为 [[ 0., 0., -1.],[ 0., 1., 0.],[ 1., 0., 0.]]
@AlexanderCska 我认为这个约定并不重要，因为翻译矩阵是一组从一个向量到另一个向量的影响因素。所选答案确实具有旋转矩阵的正确定义。

【解决方案3】：

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

【讨论】：

这听起来更像是评论而不是答案。
对不起，由于声誉低，我无法发表评论:(

【解决方案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

【讨论】：

【解决方案5】：

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

特维辛，马库斯

【讨论】：