从Eigen::AngleAxisd到Eigen::Vector3d的转换

Question

我有一个从旋转向量到旋转矩阵的转换，方法是：

Eigen::Vector3d rotVec(-0.67925191, -1.35850382, -2.03775573);
Eigen::AngleAxisd rotAA(rotVec.norm(), rotVec.normalized());
Eigen::Matrix3d rotationMatrix = rotAA.matrix();

然后我想做从矩阵到向量的反向转换，我所做的是：

Eigen::AngleAxisd rotvec2(rotationMatrix);

然而，我还无法将Eigen::AngleAxisd转换回Eigen::Vector3d，而且AngleAxisd本身似乎只有一个角度和轴成员函数。

有人知道我们该怎么做吗？

Answer 1

与angle()和axis()相比，AngleAxis具有更多的成员函数。fromRotationMatrix(const MatrixBase< Derived > &mat)或AngleAxis(const MatrixBase< Derived > &mat)将执行转换回的第一步。请参阅：https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html

然后使用乘积angle() * axis()返回原始向量。

#include <Eigen/Dense>
#include <cstdlib>

int main()
{
    using namespace Eigen;
    using T = double;

    Vector3<T> const rotVec{-0.67925191, -1.35850382, -2.03775573};
    AngleAxis<T> const rotAA{rotVec.norm(), rotVec.normalized()};
    Matrix3<T> const rotMat{rotAA.toRotationMatrix()};

#if 0
    AngleAxis<T> rotAA2{};
    rotAA2.fromRotationMatrix(rotMat);
#else
    AngleAxis<T> const rotAA2{rotMat};
#endif

    Vector3<T> const rotVec2{rotAA2.angle() * rotAA2.axis()};
    bool const is_valid = rotVec.isApprox(rotVec2);
    return is_valid ? EXIT_SUCCESS : EXIT_FAILURE;
}