本文基于SLAM十四讲经典Sophus示例代码
Sophus代码逐模块拆解：SO(3)、SE(3)李群李代数对应数学公式
把每一行代码和李群、李代数数学公式一一对应，方便学习SLAM位姿表示与扰动模型。
符号约定（与Sophus库保持一致）
- SO(3)\boldsymbol{SO}(3)SO(3)：三维旋转矩阵群，群元素为旋转矩阵R\boldsymbol{R}R
- so(3)\mathfrak{so}(3)so(3)：SO(3)\boldsymbol{SO}(3)SO(3)对应的李代数，元素为三维旋转向量ϕ\boldsymbol{\phi}ϕ
- SE(3)\boldsymbol{SE}(3)SE(3)：三维刚体变换群，群元素为齐次变换矩阵
T=[Rt0⊤1] \boldsymbol{T}= \begin{bmatrix} \boldsymbol{R} & \boldsymbol{t} \\ \boldsymbol{0}^\top & 1 \end{bmatrix}T=[R0⊤t1] - se(3)\mathfrak{se}(3)se(3)：SE(3)\boldsymbol{SE}(3)SE(3)对应的李代数，六维向量
ξ=[ρϕ] \boldsymbol{\xi}= \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{bmatrix}ξ=[ρϕ]
ρ\boldsymbol{\rho}ρ为平移对应部分，ϕ\boldsymbol{\phi}ϕ为旋转对应部分； - ∧\wedge∧：Hat算子（向量→反对称矩阵）；∨\vee∨：Vee算子（反对称矩阵→向量）
- exp(⋅)\exp(\cdot)exp(⋅)：指数映射（李代数→\to→李群）；log(⋅)\log(\cdot)log(⋅)：对数映射（李群→\to→李代数）
1. 初始化旋转、平移基础数据
| 代码 | 数学含义 |
|---|---|
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); | 构造绕ZZZ轴旋转90∘90^\circ90∘的旋转矩阵：R=exp((θn)∧),θ=π2,n=(0,0,1)⊤\boldsymbol{R}=\exp\big((\theta \boldsymbol{n})^\wedge\big),\quad \theta=\frac{\pi}{2},\boldsymbol{n}=(0,0,1)^\topR=exp((θn)∧),θ=2π,n=(0,0,1)⊤ |
Quaterniond q(R); | 旋转矩阵转为单位四元数：q=[cos(θ/2)nsin(θ/2)]\boldsymbol{q}=\begin{bmatrix}\cos(\theta/2) \\ \boldsymbol{n}\sin(\theta/2)\end{bmatrix}q=[cos(θ/2)nsin(θ/2)] |
Vector3d t(1, 0, 0); | 平移向量：t=(1,0,0)⊤\boldsymbol{t}=(1,0,0)^\topt=(1,0,0)⊤ |
2.SO(3)\boldsymbol{SO}(3)SO(3)李群元素构造
| 代码 | 数学公式 |
|---|---|
Sophus::SO3d SO3_R(R); | 由旋转矩阵直接构造李群元素：R∈SO(3)\boldsymbol{R}\in \boldsymbol{SO}(3)R∈SO(3) |
Vector3d v(0, 0, M_PI / 2);<br>Sophus::SO3d SO3_v = Sophus::SO3d::exp(v); | 李代数通过指数映射得到李群：Rv=exp(v∧),v=(0,0,π/2)⊤\boldsymbol{R}_v=\exp\big(\boldsymbol{v}^\wedge\big),\quad \boldsymbol{v}=(0,0,\pi/2)^\topRv=exp(v∧),v=(0,0,π/2)⊤ |
Sophus::SO3d SO3_q(q); | 由单位四元数构造旋转矩阵，得到SO(3)\boldsymbol{SO}(3)SO(3)元素 |
3.SO(3)\boldsymbol{SO}(3)SO(3)核心运算：对数映射、Hat/Vee、扰动更新
| 代码 | 数学公式 |
|---|---|
SO3_R.matrix() | 输出旋转矩阵本身R∈R3×3\boldsymbol{R}\in\mathbb{R}^{3\times3}R∈R3×3。 |
Vector3d so3 = SO3_R.log(); | 对数映射（李群→\to→李代数）： ϕ=(log(R))∨\boldsymbol{\phi}=\big(\log(\boldsymbol{R})\big)^\veeϕ=(log(R))∨（得到旋转向量），满足R=exp(ϕ∧)\boldsymbol{R}=\exp\big(\boldsymbol{\phi}^\wedge\big)R=exp(ϕ∧)。 |
Sophus::SO3<double>::hat(so3) | ∧\wedge∧(Hat) 算子（向量→\to→反对称矩阵）： ϕ∧=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]∈so(3)\boldsymbol{\phi}^\wedge=\begin{bmatrix}0 & -\phi_3 & \phi_2 \\\phi_3 & 0 & -\phi_1 \\-\phi_2 & \phi_1 & 0\end{bmatrix}\in\mathfrak{so}(3)ϕ∧=0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10∈so(3) |
Sophus::SO3<double>::vee(...) | ∨\vee∨(Vee) 算子（反对称矩阵→\to→向量）： (ϕ∧)∨=ϕ\big(\boldsymbol{\phi}^\wedge\big)^\vee=\boldsymbol{\phi}(ϕ∧)∨=ϕ 是 Hat 算子的逆运算。 |
Vector3d update_so3(1e-4, 0, 0);SO3_updated = Sophus::SO3<double>::exp(update_so3) * SO3_R; | 左乘扰动模型（微积分中的“扰动更新”）： Rnew=exp(δϕ∧)⋅R\boldsymbol{R}_{new}=\exp\big(\delta\boldsymbol{\phi}^\wedge\big)\cdot\boldsymbol{R}Rnew=exp(δϕ∧)⋅R 此处δϕ=(10−4,0,0)⊤\delta\boldsymbol{\phi}=(10^{-4},0,0)^\topδϕ=(10−4,0,0)⊤。 |
4.SE(3)\boldsymbol{SE}(3)SE(3)李群元素构造
| 代码 | 数学公式 |
|---|---|
Sophus::SE3d SE3_Rt(R, t); | 由旋转矩阵R\boldsymbol{R}R、平移t\boldsymbol{t}t构造SE(3)\boldsymbol{SE}(3)SE(3)变换矩阵：T=[Rt0⊤1]∈SE(3)\boldsymbol{T}=\begin{bmatrix}\boldsymbol{R} & \boldsymbol{t} \\\boldsymbol{0}^\top & 1\end{bmatrix}\in\boldsymbol{SE}(3)T=[R0⊤t1]∈SE(3) |
Sophus::SE3d SE3_qt(q, t); | 由四元数q\boldsymbol{q}q和平移t\boldsymbol{t}t构造变换矩阵（四元数内部转为R\boldsymbol{R}R） |
5.SE(3)\boldsymbol{SE}(3)SE(3)核心运算：对数映射、Hat/Vee、扰动更新
| 代码 | 数学公式 |
|---|---|
SE3_Rt.matrix() | 输出4×4齐次变换矩阵T\boldsymbol{T}T |
Vector6d se3 = SE3_Rt.log(); | 对数映射得到se(3)\mathfrak{se}(3)se(3)李代数：ξ=(log(T))∨=[ρϕ]∈se(3)\boldsymbol{\xi}=\big(\log(\boldsymbol{T})\big)^\vee=\begin{bmatrix}\boldsymbol{\rho}\\\boldsymbol{\phi}\end{bmatrix}\in\mathfrak{se}(3)ξ=(log(T))∨=[ρϕ]∈se(3) |
Sophus::SE3d::vee(...) | Vee算子：(ξ∧)∨=ξ\big(\boldsymbol{\xi}^\wedge\big)^\vee=\boldsymbol{\xi}(ξ∧)∨=ξ |
Vector6d update_se3; update_se3.setZero(); update_se3(0,0) = 1e-4d;<br>Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt; | SE(3)\boldsymbol{SE}(3)SE(3)左乘扰动模型：Tnew=exp(δξ∧)⋅T\boldsymbol{T}_{new}=\exp\big(\delta\boldsymbol{\xi}^\wedge\big)\cdot\boldsymbol{T}Tnew=exp(δξ∧)⋅T |
💎 核心公式汇总（SLAM后端高频）
- 指数映射（李代数 → 李群）
R=exp(ϕ∧),T=exp(ξ∧) \boldsymbol{R}=\exp(\boldsymbol{\phi}^\wedge),\quad \boldsymbol{T}=\exp(\boldsymbol{\xi}^\wedge)R=exp(ϕ∧),T=exp(ξ∧) - 对数映射（李群 → 李代数）
ϕ=(log(R))∨,ξ=(log(T))∨ \boldsymbol{\phi}=\big(\log(\boldsymbol{R})\big)^\vee,\quad \boldsymbol{\xi}=\big(\log(\boldsymbol{T})\big)^\veeϕ=(log(R))∨,ξ=(log(T))∨ - 左乘扰动更新（位姿优化主流形式）
R←exp(δϕ∧)R,T←exp(δξ∧)T \boldsymbol{R}\leftarrow\exp(\delta\boldsymbol{\phi}^\wedge)\boldsymbol{R},\quad \boldsymbol{T}\leftarrow\exp(\delta\boldsymbol{\xi}^\wedge)\boldsymbol{T}R←exp(δϕ∧)R,T←exp(δξ∧)T
补充说明：
- 旧版Sophus(
version0)：SO3 / SE3；新版Sophus(master)：SO3d / SE3d，头文件为.hpp；- Sophus中se(3)\mathfrak{se}(3)se(3)向量排布：平移ρ\boldsymbol{\rho}ρ在前，旋转ϕ\boldsymbol{\phi}ϕ在后，不要记反顺序！
