基于SLAM十四讲经典Sophus示例代码对应李代公式

本文基于SLAM十四讲经典Sophus示例代码Sophus代码逐模块拆解SO(3)、SE(3)李群李代数对应数学公式把每一行代码和李群、李代数数学公式一一对应方便学习SLAM位姿表示与扰动模型。符号约定与Sophus库保持一致SO(3)\boldsymbol{SO}(3)SO(3)三维旋转矩阵群群元素为旋转矩阵R\boldsymbol{R}Rso(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∘的旋转矩阵Rexp⁡((θ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)^\topRexp((θ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);brSophus::SO3d SO3_v Sophus::SO3d::exp(v);李代数通过指数映射得到李群Rvexp⁡(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))∨得到旋转向量满足Rexp⁡(ϕ∧)\boldsymbol{R}\exp\big(\boldsymbol{\phi}^\wedge\big)Rexp(ϕ∧)。Sophus::SO3double::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​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​​∈so(3)Sophus::SO3double::vee(...)∨\vee∨(Vee) 算子反对称矩阵→\to→向量(ϕ∧)∨ϕ\big(\boldsymbol{\phi}^\wedge\big)^\vee\boldsymbol{\phi}(ϕ∧)∨ϕ是 Hat 算子的逆运算。Vector3d update_so3(1e-4, 0, 0);SO3_updated Sophus::SO3double::exp(update_so3) * SO3_R;左乘扰动模型微积分中的“扰动更新”Rnewexp⁡(δϕ∧)⋅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}R5.SE(3)\boldsymbol{SE}(3)SE(3)核心运算对数映射、Hat/Vee、扰动更新代码数学公式SE3_Rt.matrix()输出4×4齐次变换矩阵T\boldsymbol{T}TVector6d 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;brSophus::SE3d SE3_updated Sophus::SE3d::exp(update_se3) * SE3_Rt;SE(3)\boldsymbol{SE}(3)SE(3)左乘扰动模型Tnewexp⁡(δξ∧)⋅T\boldsymbol{T}_{new}\exp\big(\delta\boldsymbol{\xi}^\wedge\big)\cdot\boldsymbol{T}Tnew​exp(δξ∧)⋅T 核心公式汇总SLAM后端高频指数映射李代数 → 李群Rexp⁡(ϕ∧),Texp⁡(ξ∧) \boldsymbol{R}\exp(\boldsymbol{\phi}^\wedge),\quad \boldsymbol{T}\exp(\boldsymbol{\xi}^\wedge)Rexp(ϕ∧),Texp(ξ∧)对数映射李群 → 李代数ϕ(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头文件为.hppSophus中se(3)\mathfrak{se}(3)se(3)向量排布平移ρ\boldsymbol{\rho}ρ在前旋转ϕ\boldsymbol{\phi}ϕ在后不要记反顺序