R.align_vectors()Bug

UserWarning: Optimal rotation is not uniquely or poorly defined for the given sets of vectors.解决方案

关于R.align_vectors()的使用说明：

在使用scipy.spatial.transform.Rotation.align_vectors的函数时，提示出现UserWarning: Optimal rotation is not uniquely or poorly defined for the given sets of vectors.的错误。

由此我开始质疑利用这个函数计算得到的两个三维向量的旋转矩阵是否正确。

代码如下：

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

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 = np.array([2, 3, 2.5]).reshape(1,3)
vec2 = np.array([-3, 1, -3.4]).reshape(1,3)

mat = rotation_matrix_from_vectors(vec2, vec1)

print(mat)
# 有错误！！！！
print(R.align_vectors(vec2, vec1)[0].as_matrix())    

输出结果为：

可以看到两种方法有很大不一致，再经过C++中Eigen库的计算

#include <iostream>
#include <Eigen/Dense>

Eigen::Matrix3d GetRotationMatrixBetweenTwo3DVector1(Eigen::Vector3d vector_before, Eigen::Vector3d vector_after)
{
    Eigen::Matrix3d rotation_matrix;

    rotation_matrix = Eigen::Quaterniond::FromTwoVectors(vector_before, vector_after).toRotationMatrix();
    return rotation_matrix;
}


int main()
{

    Eigen::Vector3d vector_before{ 2, 3, 2.5 };
    Eigen::Vector3d vector_after{ -3, 1, -3.4 };

    Eigen::Matrix3d rotation_matrix;

    rotation_matrix = Eigen::Quaterniond::FromTwoVectors(vector_before, vector_after).toRotationMatrix();

    std::cout << "Rotation Matrix: " << std::endl << rotation_matrix << std::endl;
    std::cout << "Result Point" << std::endl << rotation_matrix * vector_before << std::endl;

    return 0;
}

得到结果：

可以看到scipy.spatial.transform.Rotation.align_vectors函数计算错误。

在后面的计算中要慎用这个函数。