工程数学---Eigen库（C++唯一标配线性代数库）

Eigen是C++唯一标配线性代数库 ，专为机器人学、计算机视觉、SLAM、三维重建等领域设计------轻量、头文件-only、无依赖、运算效率媲美商业库，是写位姿估计、坐标变换、相机投影、点云处理的核心工具。

一、Eigen库核心概述

1. 定位

纯头文件线性代数库，无需编译链接，直接#include <Eigen/...>使用。

2. 机器人/视觉核心用途

• 三维旋转/平移（位姿）表示与变换
• 相机针孔模型、投影/反投影、单应/本质矩阵
• SLAM李群李代数、优化求解
• 点云坐标变换、特征点运算
• 机器人运动学、雅可比矩阵、坐标系统一

3. 核心数据类型规则

Eigen所有类型遵循：Matrix<Scalar, Rows, Cols>

• Scalar：数值类型（机器人/视觉强制用double，精度要求高）
• Rows/Cols：维度（静态维度优先，编译期确定，速度最快）

---

二、基础核心：矩阵与向量

这是所有几何运算的基础，线性代数基础 ，最常用的静态矩阵/向量。

1. 常用预定义类型（背下来，直接用）

类型	含义	数学对应	应用场景
Matrix3d	3×3双精度矩阵	R∈R3×3\boldsymbol{R} \in \mathbb{R}^{3×3}R∈R3×3	旋转矩阵、相机内参
Matrix4d	4×4双精度矩阵	T∈R4×4\boldsymbol{T} \in \mathbb{R}^{4×4}T∈R4×4	齐次变换矩阵（位姿）
Vector3d	3维双精度列向量	v∈R3\boldsymbol{v} \in \mathbb{R}^3v∈R3	三维点、平移向量、轴向量
Vector4d	4维双精度列向量	齐次坐标	三维点齐次表示
Quaterniond	双精度四元数	单位四元数	旋转最优表示（SLAM核心）

2. 基础操作

（1）初始化

#include <Eigen/Core>
using namespace Eigen;

// 1. 零矩阵/单位矩阵（数学：单位矩阵I是乘法单位元）
Matrix3d R = Matrix3d::Zero();    // 全0矩阵
Matrix3d I = Matrix3d::Identity();// 单位矩阵（旋转矩阵初始值）
Matrix4d T = Matrix4d::Identity();// 齐次矩阵默认初始化（必须用单位矩阵）

// 2. 直接赋值（旋转矩阵示例）
Matrix3d R;
R << 1,0,0,
     0,1,0,
     0,0,1;

（2）元素访问

Vector3d t(1,2,3); 
double x = t(0);   // 向量访问：索引0开始
double r00 = R(0,0);// 矩阵访问：(行,列)

（3）基础线性运算（数学原理）

运算	代码	数学原理	应用场景
矩阵乘法	A * B	线性变换复合（顺序不可颠倒）	旋转复合、坐标变换
矩阵转置	A.transpose()	AT\boldsymbol{A}^TAT	旋转矩阵逆（R−1=RTR^{-1}=R^TR−1=RT）
矩阵求逆	A.inverse()	A−1\boldsymbol{A}^{-1}A−1	逆坐标变换、相机内参逆
点积	a.dot(b)	a⋅b\boldsymbol{a} \cdot \boldsymbol{b}a⋅b	向量夹角、法向量计算
叉积	a.cross(b)	a×b\boldsymbol{a} \times \boldsymbol{b}a×b	求垂直向量、旋转轴

---

三、核心模块：几何变换

三维空间中，刚体的位姿 = 旋转 + 平移 ，Eigen的Eigen/Geometry模块封装了所有旋转表示，数学原理=李群SO(3)/SE(3)。

前置数学：旋转的数学本质

三维旋转属于特殊正交群SO(3)，满足两个核心性质：

1. 正交性：RTR=I\boldsymbol{R}^T \boldsymbol{R} = \boldsymbol{I}RTR=I → 逆矩阵=转置
2. 行列式：det⁡(R)=1\det(\boldsymbol{R}) = 1det(R)=1 → 保体积、无缩放

刚体位姿属于特殊欧几里得群SE(3) ，用4×4齐次变换矩阵表示。

---

模块1：旋转的4种表示

1. 旋转矩阵 Matrix3d（最基础）

• 数学定义：3×3正交矩阵，SO(3)的标准表示
• 优点 ：直观、复合方便；缺点：冗余（9个参数表示3个自由度）
• 核心用法：坐标变换、旋转复合

#include <Eigen/Geometry>
// 单位旋转矩阵
Matrix3d R = Matrix3d::Identity();

2. 轴角 AngleAxisd（旋转的本质表示）

• 数学原理 ：罗德里格斯旋转公式 （Rodrigues' rotation formula）

  任何旋转都可以用「1个旋转轴 + 1个旋转角 」唯一表示：
  R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧\boldsymbol{R} = \cos\theta \boldsymbol{I} + (1-\cos\theta)\boldsymbol{n}\boldsymbol{n}^T + \sin\theta \boldsymbol{n}^\wedgeR=cosθI+(1−cosθ)nnT+sinθn∧

  （n\boldsymbol{n}n：单位轴向量，θ\thetaθ：旋转角）

  用 1个旋转轴 + 1个旋转角度 表示任意三维旋转。

• Eigen类型 ：AngleAxisd

• 用法：单轴旋转、欧拉角转旋转矩阵

// 绕X轴旋转90度（PI/2弧度）
AngleAxisd rot_x(M_PI/2, Vector3d::UnitX());  //90度
// 绕Y/Z轴
AngleAxisd rot_y(M_PI/3, Vector3d::UnitY());   //60度
AngleAxisd rot_z(M_PI/4, Vector3d::UnitZ());   //45度

// 轴角 ↔ 旋转矩阵（自动隐式转换）
Matrix3d R = rot_z * rot_y * rot_x; // 旋转复合：后执行×先执行

构造函数参数

AngleAxisd(
    const double& angle,      // 参数1：旋转角（弧度）
    const Vector3d& axis      // 参数2：旋转轴（3D单位向量）
);

参数1：angle（旋转角）

• 单位：必须是 弧度（rad）
• 不能直接填角度（90°不行，必须写 M_PI/2）
• 旋转方向遵循 右手定则

参数2：axis（旋转轴）

必须是 3维单位向量 （长度=1），表示绕哪根轴旋转。

Eigen 已经给你写好了现成的：

Vector3d::UnitX()   // 绕 X 轴
Vector3d::UnitY()   // 绕 Y 轴
Vector3d::UnitZ()   // 绕 Z 轴

也可以自己写任意轴：

Vector3d(1,0,0)  // X轴
Vector3d(0,1,0)  // Y轴
Vector3d(0,0,1)  // Z轴

3. 四元数 Quaterniond（旋转最优表示，SLAM必备）

• 数学原理 ：单位四元数 q=w+xi+yj+zkq = w + xi + yj + zkq=w+xi+yj+zk，满足 w2+x2+y2+z2=1w^2+x^2+y^2+z^2=1w2+x2+y2+z2=1
  4个参数表示3个旋转自由度，无万向锁、运算快、数值稳定
• Eigen坑点 ：存储顺序为 x,y,z,w，初始化用w,x,y,z
• 核心用法：SLAM后端优化、旋转插值、相机位姿存储

// 1. 旋转矩阵 ↔ 四元数
Matrix3d R;
Quaterniond q(R);        // 矩阵→四元数
Matrix3d R2 = q.matrix();// 四元数→矩阵

// 2. 四元数旋转复合（比矩阵更快）
Quaterniond q1, q2;
Quaterniond q_total = q2 * q1; // 复合顺序：先q1，后q2

// 3. 用四元数旋转向量
Vector3d v(1,0,0);
Vector3d v_rot = q * v; // 数学：q v q^{-1}，Eigen封装好

4. 欧拉角 eulerAngles()（最直观）

• 数学原理 ：将旋转分解为三次绕固定轴的旋转 （机器人/视觉用Z-Y-X固定轴 ）
  总旋转：R=Rz(θz)Ry(θy)Rx(θx)\boldsymbol{R} = R_z(\theta_z) R_y(\theta_y) R_x(\theta_x)R=Rz(θz)Ry(θy)Rx(θx)
• 缺点：万向锁（奇异点），仅用于人机交互
• Eigen用法：把抽象的旋转矩阵，转换成人类能看懂的 x/y/z 三个旋转角度，方便显示、调试、输入姿态

Matrix3d R;
// 解算Z-Y-X欧拉角：参数(2,1,0)对应Z-Y-X轴
Vector3d rpy = R.eulerAngles(2,1,0); 
double rx = rpy(2); // X轴(Roll)
double ry = rpy(1); // Y轴(Pitch)
double rz = rpy(0); // Z轴(Yaw)

---

模块2：齐次变换矩阵 Matrix4d（机器人位姿核心）

1. 数学定义（SE(3)）

4×4齐次矩阵，同时表示旋转+平移 ，是机器人位姿的标准表示：
T=[R3×3t3×101×31] \boldsymbol{T} = \begin{bmatrix} \boldsymbol{R}{3×3} & \boldsymbol{t}{3×1} \\ 0_{1×3} & 1 \end{bmatrix} T=[R3×301×3t3×11]

• R\boldsymbol{R}R：旋转矩阵
• t\boldsymbol{t}t：平移向量
• 最后一行[0,0,0,1]：齐次坐标固定格式

2. Eigen核心用法

// 1. 初始化（必须用单位矩阵）
Matrix4d T = Matrix4d::Identity();

// 2. 赋值旋转矩阵+平移向量
Matrix3d R;
Vector3d t(1,2,3);
T.block<3,3>(0,0) = R;  // 左上角3×3：旋转矩阵
T.block<3,1>(0,3) = t;  // 第4列前3行：平移向量

// 3. 提取旋转+平移（逆运算）
Matrix3d R_extract = T.block<3,3>(0,0);
Vector3d t_extract = T.block<3,1>(0,3);

3. 坐标变换

• 数学原理 ：齐次坐标变换 pworld=Tworldcamera⋅pcamera\boldsymbol{p}{world} = \boldsymbol{T}{world}^{camera} \cdot \boldsymbol{p}_{camera}pworld=Tworldcamera⋅pcamera
• 向量转齐次坐标 ：三维点p(x,y,z)\boldsymbol{p}(x,y,z)p(x,y,z) → 齐次点ph(x,y,z,1)\boldsymbol{p_h}(x,y,z,1)ph(x,y,z,1)

// 相机坐标系下的点
Vector3d p_cam(1,0,0);
// 转为齐次坐标
Vector4d p_cam_h;
p_cam_h << p_cam, 1.0;
// 相机→世界坐标系变换
Matrix4d T_world_cam;
Vector4d p_world_h = T_world_cam * p_cam_h;
// 转回三维点
Vector3d p_world = p_world_h.head<3>();

---

四、计算机视觉专用Eigen知识点

视觉核心是针孔相机模型，Eigen完美封装所有投影运算。

1. 相机内参矩阵 Matrix3d K

数学原理（针孔相机模型）

内参矩阵是相机固有参数，将三维相机坐标投影到二维像素坐标：
K=[fx0cx0fycy001] \boldsymbol{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= fx000fy0cxcy1

• fx,fyf_x,f_yfx,fy：焦距，cx,cyc_x,c_ycx,cy：主点坐标

Eigen用法

// 定义相机内参
Matrix3d K;
K << 500, 0, 320,
     0, 500, 240,
     0, 0, 1;

2. 三维点→二维像素投影（视觉核心）

数学公式

u=fxX+cxZZ,v=fyY+cyZZ u = \frac{f_x X + c_x Z}{Z}, \quad v = \frac{f_y Y + c_y Z}{Z} u=ZfxX+cxZ,v=ZfyY+cyZ

矩阵形式：[uv1]=K⋅[XYZ]\begin{bmatrix}u\\v\\1\end{bmatrix} = \boldsymbol{K} \cdot \begin{bmatrix}X\\Y\\Z\end{bmatrix} uv1 =K⋅ XYZ

Eigen代码

// 相机坐标系3D点
Vector3d P_cam(3,4,10);
// 投影到像素
Vector3d uv = K * P_cam;
double u = uv(0)/uv(2); // 归一化
double v = uv(1)/uv(2);

3. 视觉核心矩阵

矩阵类型	Eigen类型	数学原理	应用场景
单应矩阵	Matrix3d	平面投影变换	图像拼接、AR
本质矩阵E	Matrix3d	对极约束（标定相机）	SLAM位姿估计
基础矩阵F	Matrix3d	对极约束（未标定相机）	特征点匹配

---

五、机器人学专用Eigen知识点

1. 坐标变换链式法则（机器人多坐标系）

机器人有基坐标系、工具坐标系、相机坐标系 ，变换遵循：
TAB=TAC⋅TCB \boldsymbol{T}{AB} = \boldsymbol{T}{AC} \cdot \boldsymbol{T}_{CB} TAB=TAC⋅TCB

• 含义：C→A变换 = B→A变换 + C→B变换
• 乘法顺序绝对不能颠倒

2. 李群李代数（SLAM核心优化）

Eigen直接支持SO(3)/SE(3)与李代数的转换，是视觉SLAM的核心：

// 旋转矩阵 → 李代数so3
Matrix3d R;
Vector3d so3 = Sophus::SO3d(R).log(); // 需配合Sophus库
// 李代数 → 旋转矩阵
Matrix3d R2 = Sophus::SO3d::exp(so3).matrix();

3. 雅可比矩阵（机器人运动控制）

机器人运动学求导用MatrixXd（动态矩阵），用于轨迹跟踪、逆运动学。

---

六、高级常用功能（机器人/视觉必备）

1. 线性方程组求解（Ax=b）

视觉SLAM、机器人标定必备，Eigen提供超高效求解器：

Matrix3d A;
Vector3d b, x;
x = A.colPivHouseholderQr().solve(b); // 稳定求解Ax=b

2. 块操作（高效切片）

Matrix4d T;
Matrix3d R = T.block<3,3>(0,0); // 取左上角3×3
Vector3d t = T.block<3,1>(0,3); // 取第4列前3行

3. 数值运算

double det = R.determinant(); // 行列式（旋转矩阵=1）
double norm = v.norm();       // 向量模长
Vector3d v_norm = v.normalized(); // 单位向量

---

七、综合实战

1. 参数→齐次矩阵（欧拉角→旋转矩阵→位姿）

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

struct RobotPose{
    double x,y,z,rx,ry,rz;
};

Matrix4d param2homogeneous(double x, double y, double z, double rx, double ry, double rz) {
    // 轴角→旋转复合（罗德里格斯公式）
    AngleAxisd rot_x(rx, Vector3d::UnitX());
    AngleAxisd rot_y(ry, Vector3d::UnitY());
    AngleAxisd rot_z(rz, Vector3d::UnitZ());
    Matrix3d R = rot_z * rot_y * rot_x; // Z-Y-X固定轴复合

    Matrix4d T = Matrix4d::Identity();
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = Vector3d(x,y,z);
    return T;
}

2. 齐次矩阵→参数（旋转矩阵→欧拉角）

RobotPose homogeneous2param(const Matrix4d& T) {
    RobotPose pose;
    // 提取平移
    Vector3d t = T.block<3,1>(0,3);
    pose.x = t.x(); pose.y = t.y(); pose.z = t.z();

    // 提取旋转矩阵→解算Z-Y-X欧拉角
    Matrix3d R = T.block<3,3>(0,0);
    Vector3d rpy = R.eulerAngles(2,1,0);
    pose.rx = rpy(2); pose.ry = rpy(1); pose.rz = rpy(0);
    return pose;
}

---

八、机器人/视觉Eigen避坑指南

1. 旋转复合顺序 ：矩阵乘法 = 后执行的变换 × 先执行的变换
2. 单位 ：所有角度用弧度 （MPIM_PIMPI），禁止用角度
3. 四元数顺序 ：Eigen存储x,y,z,w，初始化w,x,y,z
4. 齐次矩阵初始化 ：必须用Matrix4d::Identity()，禁止用Zero()
5. 欧拉角顺序 ：机器人/视觉固定用eulerAngles(2,1,0)（Z-Y-X）
6. 数据类型 ：强制用double，禁止用float（精度不足）

---

总结

1. 基础 ：静态矩阵/向量（Matrix3d/4d、Vector3d）
2. 旋转：4种表示（矩阵、轴角、四元数、欧拉角）+ SO(3)数学
3. 位姿：4×4齐次变换矩阵 + SE(3)数学 + 坐标变换
4. 视觉：针孔相机模型、内参矩阵、三维→二维投影
5. 机器人：坐标链式变换、李群李代数、运动学求解

---

补充数学知识点

三维向量的「反对称算子（Skew-symmetric Operator）」 ，也常被称为「帽子算子（Hat Operator）」，符号为∧\wedge∧，记为n∧\boldsymbol{n}^\wedgen∧（也有大量机器人学/视觉文献写成[n]×[\boldsymbol{n}]_\times[n]×，两种写法完全等价），是三维旋转、李群李代数、机器人位姿计算中最核心的符号之一。

---

1. 数学定义：向量 → 反对称矩阵

对于任意三维列向量 n=[nxnynz]\boldsymbol{n} = \begin{bmatrix} n_x \\ n_y \\ n_z \end{bmatrix}n= nxnynz ，帽子算子∧\wedge∧的作用是将其转换为一个3×3的反对称（斜对称）矩阵 n∧\boldsymbol{n}^\wedgen∧，严格定义为：
n∧=[0−nznynz0−nx−nynx0] \boldsymbol{n}^\wedge = \begin{bmatrix} 0 & -n_z & n_y \\ n_z & 0 & -n_x \\ -n_y & n_x & 0 \end{bmatrix} n∧= 0nz−ny−nz0nxny−nx0

这个矩阵满足核心性质：反对称性 (n∧)T=−n∧(\boldsymbol{n}^\wedge)^T = -\boldsymbol{n}^\wedge(n∧)T=−n∧，同时迹为0、行列式为0，是三维旋转李群SO(3)\text{SO}(3)SO(3)对应的李代数so(3)\mathfrak{so}(3)so(3)的标准矩阵形式。

---

2. 本质作用：叉乘 = 矩阵乘法

这个算子最核心的价值，是把三维向量的叉乘运算，1:1等价转换为矩阵乘法 ：

对于任意两个三维向量n\boldsymbol{n}n和v\boldsymbol{v}v，恒有：
n×v=n∧⋅v \boldsymbol{n} \times \boldsymbol{v} = \boldsymbol{n}^\wedge \cdot \boldsymbol{v} n×v=n∧⋅v

直观验证例子

取n=[1,0,0]T\boldsymbol{n} = [1,0,0]^Tn=[1,0,0]T（X轴单位向量），则：
n∧=[00000−1010] \boldsymbol{n}^\wedge = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix} n∧= 0000010−10

对任意v=[x,y,z]T\boldsymbol{v} = [x,y,z]^Tv=[x,y,z]T：

• 叉乘计算：n×v=[100]×[xyz]=[0−zy]\boldsymbol{n} \times \boldsymbol{v} = \begin{bmatrix}1\\0\\0\end{bmatrix} \times \begin{bmatrix}x\\y\\z\end{bmatrix} = \begin{bmatrix}0\\-z\\y\end{bmatrix}n×v= 100 × xyz = 0−zy
• 矩阵乘法计算：n∧⋅v=[00000−1010][xyz]=[0−zy]\boldsymbol{n}^\wedge \cdot \boldsymbol{v} = \begin{bmatrix}0&0&0\\0&0&-1\\0&1&0\end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix} = \begin{bmatrix}0\\-z\\y\end{bmatrix}n∧⋅v= 0000010−10 xyz = 0−zy
  两者完全一致，完美验证了这个等价关系。

---

3. 在罗德里格斯公式中的意义

罗德里格斯旋转公式（Rodrigues' Rotation Formula） 是「轴角表示（旋转轴n\boldsymbol{n}n、旋转角θ\thetaθ）→ 旋转矩阵R\boldsymbol{R}R」的核心数学工具：
R=cos⁡θI+(1−cos⁡θ)nnT+sin⁡θn∧ \boldsymbol{R} = \cos\theta \boldsymbol{I} + (1-\cos\theta)\boldsymbol{n}\boldsymbol{n}^T + \sin\theta \boldsymbol{n}^\wedge R=cosθI+(1−cosθ)nnT+sinθn∧

这里用n∧\boldsymbol{n}^\wedgen∧的原因：

• 绕轴n\boldsymbol{n}n旋转的几何本质，是将向量分解为「平行于n\boldsymbol{n}n的分量」和「垂直于n\boldsymbol{n}n的平面分量」，平面分量的旋转需要用叉乘描述；
• 用n∧\boldsymbol{n}^\wedgen∧把叉乘转换成矩阵乘法后，整个公式统一为矩阵运算 ，可以直接在代码中实现（Eigen::AngleAxisd，内部就是通过这个公式+反对称算子计算旋转矩阵的）。

---

4. 机器人/视觉/Eigen中的实战应用

（1）Eigen库中的直接实现

Eigen为Vector3d封装了这个算子，调用.skew()方法即可直接得到反对称矩阵：

#include <Eigen/Core>
using namespace Eigen;

Vector3d n(1, 0, 0); // X轴单位向量
Matrix3d n_hat = n.skew(); // 直接得到n^∧，即上面例子中的反对称矩阵

上面的param2homogeneous函数中，AngleAxisd转旋转矩阵的底层，就是用罗德里格斯公式+这个算子实现的。

（2）李群李代数的核心桥梁

在SLAM、机器人状态估计中：

• 三维旋转属于李群SO(3)\text{SO}(3)SO(3)，其对应的李代数so(3)\mathfrak{so}(3)so(3)就是所有3×3反对称矩阵的集合；
• 帽子算子∧\wedge∧：把李代数的「向量形式」ϕ∈R3\boldsymbol{\phi} \in \mathbb{R}^3ϕ∈R3 → 「矩阵形式」ϕ∧∈so(3)\boldsymbol{\phi}^\wedge \in \mathfrak{so}(3)ϕ∧∈so(3)；
• 逆算子「vee算子∨\vee∨」：把反对称矩阵 → 三维向量，满足(ϕ∧)∨=ϕ(\boldsymbol{\phi}^\wedge)^\vee = \boldsymbol{\phi}(ϕ∧)∨=ϕ；
  这是李代数求导、BA优化、位姿估计的核心基础。

（3）其他高频场景

• 机器人运动学：坐标变换的雅可比矩阵计算
• 计算机视觉：对极几何中本质矩阵、基础矩阵的推导
• SLAM后端：旋转矩阵的扰动模型、位姿优化

---

5. 说明

1. 写法等价性 ：n∧\boldsymbol{n}^\wedgen∧和[n]×[\boldsymbol{n}]_\times[n]×完全等价，后者更直观体现「叉乘矩阵」的含义，两种写法在机器人学文献中通用；
2. 维度限制 ：该算子仅对三维向量有效，更高维度没有对应的叉乘定义，因此无此算子；
3. 逆运算 ：反对称矩阵转向量用vee算子∨\vee∨，是∧\wedge∧的逆操作，Eigen中可通过自定义函数实现。