c++ - 在 C++ 中实现 Rodrigues 的旋转公式时遇到问题

Question

我正在尝试实现一个函数，它在 3D 空间中获取两个几何 vector 并返回一个旋转矩阵，该矩阵将第一个 vector 旋转到第二个 vector 。我的函数目前使用 Rodrigues 的旋转公式来创建矩阵，但我对该公式的实现对某些输入给出了错误的答案。我手工检查了一项给出错误结果的测试的数学，而我的工作给出了相同的结果。

这是我的函数的代码:

Matrix3d rotation_matrix(Vector3d vector0, Vector3d vector1)
{
    vector0.normalize();
    vector1.normalize();
    // vector orthogonal to both inputs
    Vector3d u = vector0.cross(vector1);
    if (!u.norm())
    {
        if (vector0 == vector1)
            return Matrix3d::Identity();
        // return rotation matrix that represents 180 degree rotation
        Matrix3d m1;
        m1 << -1, 0, 0,
               0,-1, 0,
               0, 0, 1;
        return m1;
    }

    /* For the angle between both inputs:
     * 1) The sine is the magnitude of their cross product.
     * 2) The cosine equals their dot product.
     */
    // sine must be calculated using original cross product
    double sine = u.norm();
    double cosine = vector0.dot(vector1);
    u.normalize();

    double ux = u[0];
    double uy = u[1];
    double uz = u[2];
    Matrix3d cross_product_matrix;
    cross_product_matrix << 0, -uz, uy,
                            uz,   0,-ux,
                           -uy,  ux,  0;

    Matrix3d part1 = Matrix3d::Identity();
    Matrix3d part2 = cross_product_matrix * sine;
    Matrix3d part3 = cross_product_matrix*cross_product_matrix * (1 - cosine);
    return part1 + part2 + part3;
}

我将 Eigen C++ 库用于线性代数(可在此处获取): http://eigen.tuxfamily.org/index.php?title=Main_Page

感谢任何帮助。谢谢。

Answer 1

单线性版本包括使用 Eigen 的四元数:

return Matrix3d(Quaterniond::FromTwoVectors(v0,v1));