在C++实施罗德里格斯旋转公式时遇到麻烦

Trouble Implementing Rodrigues' rotation formula in C++

本文关键字：遇到 麻烦 旋转 C++ 施罗德      更新时间：2023-10-16

我试图实现一个函数，该函数在3D空间中需要两个几何向量，并返回一个旋转矩阵，将第一个向量旋转到第二个向量。我的函数目前使用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

任何帮助都是感激的。谢谢。

一个单行版本包括使用Eigen四元数:

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

如果你想从一个向量旋转到另一个，只需使用内置的"Eigen::Quaternion:: setfromtwovvectors "http://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html ac35460294d855096e9b687cadf821452

它使你所需要的和实现的更快。然后你可以打电话"Eigen::Quaternion::toRotationMatrix"，转换为矩阵。这两种运算都比较快，可能比直接Rodrigues公式快。

相关文章：