Eigen 中的 Geometry 模块详见和应用示例

Eigen 中的 Geometry 模块模块主要面向 3D/2D 几何运算，在 SLAM、机器人学、计算机视觉、图形学 中经常用到。

1. Geometry 模块概览

Eigen 的 Geometry 模块提供了多种 几何变换与姿态表示 类型，主要包括：

这些类型与 Eigen 的矩阵类无缝结合，内部也是用矩阵实现，但提供了更直观的几何操作接口。

2. 常用类型详解

2.1 Rotation2D（二维旋转）

#include <Eigen/Dense>
#include <iostream>

int main() {
    Eigen::Rotation2D<double> rot(M_PI / 4); // 旋转 45 度
    Eigen::Vector2d v(1, 0);

    Eigen::Vector2d v_rotated = rot * v;
    std::cout << "Rotated vector: " << v_rotated.transpose() << "\n";
}

• 存储的是角度（弧度）。
• 内部是一个 2×2 旋转矩阵。

2.2 AngleAxis（轴角表示）

#include <Eigen/Geometry>
#include <iostream>

int main() {
    Eigen::AngleAxisd angle_axis(M_PI/2, Eigen::Vector3d(0, 0, 1)); // 绕 Z 旋转 90°
    Eigen::Matrix3d R = angle_axis.toRotationMatrix();

    std::cout << "Rotation matrix:\n" << R << "\n";
}

• 常用来从旋转向量构造旋转。
• 可直接乘向量：v_rot = angle_axis * v;
• 方便和四元数互转：Eigen::Quaterniond q(angle_axis);

2.3 Quaternion（四元数）

#include <Eigen/Geometry>
#include <iostream>

int main() {
    Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()));
    Eigen::Vector3d v(1, 0, 0);

    Eigen::Vector3d v_rot = q * v; // 旋转向量
    std::cout << "Rotated: " << v_rot.transpose() << "\n";

    // 四元数归一化（避免累积误差）
    q.normalize();
}

• 避免万向锁，适合插值（Slerp）。
• Eigen 默认四元数存储顺序为 (x, y, z, w)，但构造时通常 (w, x, y, z)。

2.4 Transform（变换矩阵）

Eigen::Transform<Scalar, Dim, Mode>

• Dim: 2 或 3
• Mode: Affine（仿射）、Isometry（刚体）、Projective（投影）

#include <Eigen/Geometry>
#include <iostream>

int main() {
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 刚体变换
    T.rotate(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()));
    T.pretranslate(Eigen::Vector3d(1, 2, 3));

    Eigen::Vector3d p(1, 0, 0);
    Eigen::Vector3d p_transformed = T * p;

    std::cout << "Transformed point: " << p_transformed.transpose() << "\n";
}

• Isometry 确保旋转部分正交，Affine 允许缩放与错切。
• pretranslate 是先平移再已有变换，translate 则是后平移。

2.5 Translation（纯平移）

Eigen::Translation3d trans(1, 2, 3);
Eigen::Vector3d p(0, 0, 0);
std::cout << "Translated: " << (trans * p).transpose() << "\n";

• 可与旋转、仿射变换组合。

2.6 AlignedBox（包围盒）

Eigen::AlignedBox3d box(Eigen::Vector3d(0,0,0), Eigen::Vector3d(1,1,1));
std::cout << "Center: " << box.center().transpose() << "\n";
std::cout << "Contains (0.5,0.5,0.5)? " << box.contains(Eigen::Vector3d(0.5,0.5,0.5)) << "\n";

3. 综合示例：SE(3) 位姿变换

#include <Eigen/Geometry>
#include <iostream>

int main() {
    // 构造旋转（四元数）
    Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()));

    // 构造平移
    Eigen::Vector3d t(1, 0, 0);

    // 构造 SE(3) 变换（刚体）
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(q);
    T.pretranslate(t);

    // 应用变换
    Eigen::Vector3d p(1, 1, 1);
    Eigen::Vector3d p_transformed = T * p;

    std::cout << "T:\n" << T.matrix() << "\n";
    std::cout << "p' = " << p_transformed.transpose() << "\n";
}

输出：

T:
 0.7071 -0.7071 0 1
 0.7071  0.7071 0 0
      0       0 1 0
      0       0 0 1
p' = 1 1.41421 1

4. 易踩的坑

1. 四元数存储顺序：Eigen 内部是 (x, y, z, w)，打印调试时别搞错。
2. AngleAxis 与弧度：旋转角单位是弧度而非角度。
3. Transform 类型选择：刚体变换用 Isometry，否则可能引入缩放。
4. 旋转矩阵正交化：累积运算会引入误差，需要 Eigen::Quaterniond(q).normalized() 或 Eigen::JacobiSVD 正交化。
5. 左乘 / 右乘顺序：pretranslate 与 translate 区别很大，变换顺序要严格控制。

5.应用示例

应用 1：两个姿态之间的插值（Slerp）

常用于相机轨迹平滑、动画插值、IMU 外参插值等。

#include <Eigen/Geometry>
#include <iostream>

int main() {
    Eigen::Quaterniond q1(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ()));
    Eigen::Quaterniond q2(Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ()));

    double t = 0.5; // 插值比例
    Eigen::Quaterniond q_interp = q1.slerp(t, q2);

    Eigen::Vector3d v(1, 0, 0);
    std::cout << "Interpolated rotation result: " << (q_interp * v).transpose() << "\n";
}

特点：Slerp 是球面线性插值，旋转轨迹平滑且恒定角速度。

应用 2：从旋转矩阵提取欧拉角

适用于姿态解析（比如 roll-pitch-yaw），方便调试。

#include <Eigen/Geometry>
#include <iostream>

int main() {
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(M_PI/3, Eigen::Vector3d::UnitZ());

    Eigen::Vector3d euler = R.eulerAngles(2, 1, 0); // ZYX 顺序
    std::cout << "Yaw-Pitch-Roll (rad): " << euler.transpose() << "\n";
}

注意：欧拉角存在万向锁，建议只在解析调试时使用。

应用 3：点云坐标系变换

用 Eigen::Isometry3d 做 SE(3) 变换，非常适合点云、传感器数据坐标系转换。

#include <Eigen/Geometry>
#include <iostream>
#include <vector>

int main() {
    // 构造一个 SE(3) 变换
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()));
    T.pretranslate(Eigen::Vector3d(1, 2, 0));

    // 点云
    std::vector<Eigen::Vector3d> cloud = {
        {1, 0, 0}, {0, 1, 0}, {-1, 0, 0}
    };

    // 变换后的点云
    for (auto &p : cloud) {
        Eigen::Vector3d p_trans = T * p;
        std::cout << "Point transformed: " << p_trans.transpose() << "\n";
    }
}

场景：SLAM 中从传感器坐标系到世界坐标系的变换。

应用 4：计算两个姿态的相对位姿

SLAM 后端优化、机器人路径规划、视觉重定位常用。

#include <Eigen/Geometry>
#include <iostream>

int main() {
    // 位姿 A
    Eigen::Isometry3d T_a = Eigen::Isometry3d::Identity();
    T_a.rotate(Eigen::AngleAxisd(M_PI/6, Eigen::Vector3d::UnitZ()));
    T_a.pretranslate(Eigen::Vector3d(1, 0, 0));

    // 位姿 B
    Eigen::Isometry3d T_b = Eigen::Isometry3d::Identity();
    T_b.rotate(Eigen::AngleAxisd(M_PI/3, Eigen::Vector3d::UnitZ()));
    T_b.pretranslate(Eigen::Vector3d(2, 1, 0));

    // 相对变换（B 在 A 坐标系下的位姿）
    Eigen::Isometry3d T_ab = T_a.inverse() * T_b;

    std::cout << "Relative transform:\n" << T_ab.matrix() << "\n";
}

解释：
T_a.inverse() * T_b = 先从世界系回到 A 系，再从 A 系到 B 系。