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

Question

我正在尝试实现一个函数，该函数采用 3D 中的两个几何向量 space 和 returns 一个将第一个向量旋转到第二个向量的旋转矩阵。我的函数目前使用 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::Quaternion::setFromTwoVectors" http://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#ac35460294d855096e9b687cadf821452

它使您真正需要的东西和实施速度更快。然后你可以打电话 "Eigen::Quaternion::toRotationMatrix" ，转换为矩阵。这两种操作都相当快，并且可能比直接罗德里格斯公式更快。

Answer 2

一个线性版本包括使用 Eigen 的四元数：

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

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

Trouble Implementing Rodrigues' rotation formula in C++

c++

geometry

rotation

eigen

rotational-matrices