从 Eigen::Isometry2d 获取旋转角度

Question

我正在使用 Eigen 库，特别是 2d 等轴测图来表示 2D 世界中对象的姿势。

如您所知，这包含三个组成部分：x 和 y 位置以及角度 theta 旋转。

提取位置分量很简单：

Eigen::Isometry2d t;
double x = t.translation().x();
double y = t.translation().y();

对于旋转，可以使用如下公式：

double theta = atan2(t(1, 0), t(0, 0));

这个已经测试过了，可以正常使用，但是一直写有点不方便。因此，我的问题是，是否有人知道通过调用 Eigen 方法获得 theta 的更好方法。

Answer 1

您可以从变换的线性部分构造一个 Rotation2Dd 对象（如果您更喜欢标量，请使用 .angle() 提取角度）：

double theta = Eigen::Rotation2Dd(t.linear()).angle();