70_附录_可视化与应用资源
附录：李群与微分几何——可视化·直觉·应用资源全指南
本指南为已完成纯数学理论学习的机器人工程师提供"最后一公里"资源——将抽象公式转化为可操作的几何直觉与工程能力。六个专题共筛选出约120个高质量资源，覆盖交互式可视化、名师视频、经典博客、开源项目源码与中文社区精华，帮助学生在"看到公式就能画出图、写出代码"的层面真正理解这批数学工具。
专题1：光滑流形的一般理论
1. 核心可视化资源
| 名称 |
链接 |
类型 |
核心价值 |
推荐 |
| Keenan Crane — Discrete Differential Geometry (CMU) |
https://brickisland.net/ddg-web/ |
课程+视频+讲义 |
三维几何处理视角的微分几何，可视化标杆：外代数、曲率、联络、Hodge分解全配精美图示与编程作业 |
★★★★★ |
| Albert Chern — DDG Slides (UCSD) |
https://cseweb.ucsd.edu/~alchern/teaching/DDG.pdf |
讲义PDF |
外微分形式的slides含**HackerNews多人推荐的出色图示**，Houdini编程作业工程感强 |
★★★★★ |
| DFormPy — 微分形式可视化Python库 |
https://github.com/Indivicivet/dformpy_indivicivet |
交互工具 |
提供0/1/2-形式和向量场在R²上的**交互式GUI**，支持外微分、楔积、Hodge算子图形化操作 |
★★★★ |
| scikit-learn Manifold Learning文档 |
https://scikit-learn.org/stable/modules/manifold.html |
文档+代码 |
t-SNE/UMAP/Isomap/LLE的S-curve可视化，工程师理解"高维→低维流形"的最佳实践入口 |
★★★★ |
2. 必看视频清单
| 名称 |
链接 |
核心价值 |
推荐 |
| Keenan Crane DDG YouTube全套 |
通过 https://brickisland.net/ddg-web/ 链接 |
CMU离散微分几何完整视频，强调几何直觉，视觉效果一流 |
★★★★★ |
| 3Blue1Brown — Essence of Calculus + Linear Algebra |
https://www.3blue1brown.com/ |
虽无专门微分几何系列，但其切线、导数、线性变换几何意义的动画是理解切空间的**最佳先修** |
★★★★ |
| 梁灿彬《微分几何入门与广义相对论》(Bilibili 118讲) |
https://www.bilibili.com/video/BV1qF411t72r/ |
北师大经典课程1080p修复版，中文微分几何入门标杆 |
★★★★★ |
3. 优秀博客与文章
| 名称 |
链接 |
核心价值 |
推荐 |
| Tristan Needham《Visual Differential Geometry and Forms》 |
https://www.vdgf.space/ |
**235幅手绘图**把几何放回微分几何；第五幕首次以直觉方式引入微分形式。Frank Morgan评"I know nothing like it" |
★★★★★ |
| Dan Piponi — On the Visualisation of Differential Forms |
http://yaroslavvb.com/papers/notes/piponi-on.pdf |
"把向量当针、1-形式当洋葱"的经典比喻，用MTW图像化语言讲解Stokes定理 |
★★★★★ |
| Duarte Maia — Visualization of Differential Forms |
https://math.uchicago.edu/~dmaia/documents/visualizing_diff_forms.pdf |
将k-形式可视化为"超平面族"，用"边界的边界为空"直觉解释dd=0 |
★★★★★ |
| Manifolds: A Gentle Introduction (bjlkeng) |
https://bjlkeng.io/posts/manifolds/ |
从ML角度入门流形，"地球表面局部看起来是平的"比喻，含代码和图示 |
★★★★ |
4. 工程应用案例
| 项目 |
链接 |
该专题知识的体现 |
| manif库 (C++ Lie理论库) |
https://github.com/artivis/manif |
配套Solà论文，header-only实现SO(2)/SO(3)/SE(2)/SE(3)全套操作与解析Jacobian，直接用于SLAM |
| Hands-on Lie-SLAM教程 |
https://shubodhs.ai/blog/2021/lie-slam/ |
使用manif库从2D SLAM出发实现SE(2)/SE(3) SAM，切空间微积分→代码的完整映射 |
5. 直觉化类比与推荐学习路径
核心类比：流形就是"弯曲的空间可以用很多张平面地图拼起来描述"。想象你是一只蚂蚁爬在苹果表面上——你眼前永远是平坦的（局部欧几里得），但走远了会发现绕回来了（全局拓扑非平凡）。每个坐标卡就是一张局部地图，转移函数就是地图之间的翻译规则。切空间是你在某一点所有可能出发方向构成的"箭头空间"——贴在苹果皮上的一个平面。向量场是在每一点各插一根箭头，流则是沿箭头跑出来的轨迹。微分形式是"方向无关的测量工具"——不管你用什么坐标，积分结果不变。
推荐路径：① 知乎"一文入门微分几何"建立中文直觉 → ② bjlkeng博客用ML视角衔接 → ③ Needham书挑读前三幕获得几何直觉 → ④ Piponi/Maia的微分形式可视化 → ⑤ Keenan Crane视频系统化 → ⑥ manif库动手编程。
6. 中文社区优质资源
| 名称 |
链接 |
类型 |
推荐 |
| 知乎：一文入门微分几何（物理人版） |
https://zhuanlan.zhihu.com/p/629852598 |
长文 |
★★★★★ |
| 知乎：从零开始学SLAM · 为啥需要李群与李代数？ |
https://zhuanlan.zhihu.com/p/47330137 |
对话体科普 |
★★★★★ |
| 高翔博客：视觉SLAM数学基础—李群与李代数 |
https://www.cnblogs.com/gaoxiang12/p/5137454.html |
博客 |
★★★★★ |
| 知乎：构造微分流形概念的动机是什么？ |
https://www.zhihu.com/question/405819860 |
问答 |
★★★★ |
专题2：Retraction理论与流形优化基础
1. 核心可视化资源
| 名称 |
链接 |
类型 |
核心价值 |
推荐 |
| Geomstats — 球面S²上梯度下降可视化 |
https://github.com/geomstats/geomstats |
Python库+论文图 |
论文Fig.1展示球面S²上黎曼梯度下降轨迹（红色曲线），notebooks可复现 |
★★★★★ |
| ManifoldsBase.jl — Retraction vs Exp Map对比图 |
https://juliamanifolds.github.io/ManifoldsBase.jl/stable/retractions/ |
文档+图示 |
Circle上exponential map与projective retraction的**直观对比图**，一目了然 |
★★★★★ |
| Ruda Zhang — 椭圆上三种Retraction对比 |
https://arxiv.org/pdf/2006.14751 |
论文Fig.1 |
Newton/projective/orthographic retraction的几何差异，直观展示"如何把步映回流形" |
★★★★★ |
| Boumal SIAM OP 2023 幻灯片 |
https://www.nicolasboumal.net/SIAMOP2023/Tutorial%20SIAM%20Optimization%202023.pdf |
幻灯片 |
含球面GD、retraction图示、PCA/SLAM应用举例的精美可视化 |
★★★★★ |
2. 必看视频清单
| 名称 |
链接 |
核心价值 |
推荐 |
| Boumal EPFL MATH-512 完整学期课程 (14周) |
https://www.nicolasboumal.net/book/#lectures |
流形优化最权威入门，含exercises solutions |
★★★★★ |
| Boumal Simons Institute 1小时浓缩讲座 |
https://www.youtube.com/watch?v=UjaoZE0GBpg |
更紧凑的入门版本 |
★★★★ |
3. 优秀博客与文章
| 名称 |
链接 |
核心价值 |
推荐 |
| Agustinus Kristiadi — Riemannian Optimization |
https://agustinus.kristia.de/blog/optimization-riemannian-manifolds/ |
从欧氏GD到黎曼GD逐步推导，含球面retraction代码、自然梯度视角，TikZ精美图示 |
★★★★★ |
| MIT VNAV Lecture 18-19: Optimization on Manifolds |
https://vnav.mit.edu/material/18-19-OptimizationOnManifold-notes.pdf |
面向机器人工程师最佳切入点——从"旋转不在欧氏空间"出发讲retraction |
★★★★★ |
| Boumal《An Introduction to Optimization on Smooth Manifolds》 |
https://www.nicolasboumal.net/book/ |
2023剑桥出版免费PDF；Ch.2含PCA/旋转同步实例，Ch.7列出所有常见流形的retraction |
★★★★★ |
4. 工程应用案例
| 项目 |
链接 |
该专题知识的体现 |
| SE-Sync (David Rosen) |
https://github.com/david-m-rosen/SE-Sync |
Stiefel流形上Riemannian trust-region方法解pose-graph SLAM，全局最优保证 |
| Pymanopt |
https://github.com/pymanopt/pymanopt |
Python版Manopt，examples含dominant_eigenvector(球面优化=PCA)等实例 |
| Geoopt — PyTorch黎曼优化 |
https://github.com/geoopt/geoopt |
ManifoldTensor/RiemannianAdam可直接嵌入nn.Module训练循环 |
| Geomstats — 机器人SE(3)轨迹插值 |
https://github.com/geomstats/geomstats |
含robotics模块实现机械臂轨迹在SE(3)上的测地线插值 |
5. 直觉化类比与推荐学习路径
核心类比：想象你站在地球表面（球面流形），想走到山谷最低点。欧氏梯度下降会让你沿直线穿过地球内部，但你必须待在球面上。Retraction就是一种"投影回球面"的策略：先在脚下的切平面上迈一步（黎曼梯度方向），再用某种方式"按"回球面。Exponential map是最精确的做法（沿测地线走），但计算昂贵；retraction是更便宜的近似——只要方向对（一阶条件满足），按回去的位置差一点也无妨。PCA就是Grassmann流形上的优化：找k维子空间最大化方差，搜索空间是所有k维子空间构成的Grassmann流形。
推荐路径：① CSDN"全网最通俗版本"建立中文直觉 → ② MIT VNAV讲义理解机器人动机 → ③ Boumal SIAM Tutorial视频+幻灯片 → ④ Pymanopt跑球面PCA示例 → ⑤ SE-Sync论文理解SLAM中的工业级应用。
6. 中文社区优质资源
| 名称 |
链接 |
类型 |
推荐 |
| 知乎：邓康康"黎曼优化的一点理解" |
https://zhuanlan.zhihu.com/p/85551580 |
文章 |
★★★★★ |
| CSDN：流形优化全网最通俗版本详解 |
https://blog.csdn.net/weixin_39274659/article/details/115735867 |
博客 |
★★★★★ |
| 知乎：读书笔记·黎曼流形优化及其应用 |
https://zhuanlan.zhihu.com/p/669363301 |
文章 |
★★★★ |
| Awesome Riemannian Optimization |
https://github.com/andyjm3/Awesome-Riemannian-Optimization |
论文列表 |
★★★★★ |
专题3：李群基础与SO(3)/SE(3)专论
1. 核心可视化资源
| 名称 |
链接 |
类型 |
核心价值 |
推荐 |
| 3Blue1Brown — 四元数可视化 (立体投影) |
https://www.3blue1brown.com/lessons/quaternions |
视频+文字 |
用立体投影将4D四元数映射到3D，从"线境人"类比逐步建立维度直觉 |
★★★★★ |
| 3Blue1Brown — 四元数与3D旋转交互讲解 |
https://www.3blue1brown.com/lessons/quaternions-and-3d-rotation |
交互视频 |
解释q和-q双覆盖、四元数乘法的几何意义 |
★★★★★ |
| Ben Eater — 四元数交互探索 |
https://eater.net/quaternions |
交互网页 |
可拖拽操纵四元数旋转，实时感知四元数→旋转映射 |
★★★★★ |
| GeoGebra — Rodrigues旋转可视化 |
https://www.geogebra.org/m/yt8hanyy |
交互3D |
可拖拽的轴-角旋转过程演示 |
★★★★★ |
2. 必看视频清单
| 名称 |
链接 |
核心价值 |
推荐 |
| Joan Solà — "Lie Theory for the Roboticist" |
https://youtu.be/QR1p0Rabuww |
Micro Lie Theory作者本人讲座，manif官方推荐 |
★★★★★ |
| 高翔《视觉SLAM十四讲》(Bilibili) |
https://www.bilibili.com/video/BV16t411g7FR/ |
第4讲专讲李群李代数+Sophus实践，中文SLAM入门第一课 |
★★★★★ |
| Modern Robotics — POE正运动学 |
https://modernrobotics.northwestern.edu/nu-gm-book-resource/4-1-1-product-of-exponentials-formula-in-the-space-frame/ |
逐步演示SE(3)指数映射链T(θ)=e^{[S₁]θ₁}·e^{[S₂]θ₂}·...·M |
★★★★★ |
3. 优秀博客与文章
| 名称 |
链接 |
核心价值 |
推荐 |
| Joan Solà — A Micro Lie Theory for State Estimation |
https://arxiv.org/abs/1812.01537 |
SLAM李群入门第一推荐，仅17页讲透Lie group/algebra/exp/log/adjoint/Jacobian |
★★★★★ |
| Ethan Eade — Lie Groups for 2D and 3D Transformations |
https://www.ethaneade.com/lie.pdf |
极简风格的SO(3)/SE(3)公式大全，实现导向 |
★★★★★ |
| Myron Phan — Why Circles are at the Heart of Robotics |
https://medium.com/@myronphan/intuition-for-lie-groups-for-robotics-b87167db0f4a |
零公式纯直觉：用"圆"比喻李群、切线比喻李代数 |
★★★★★ |
| manif Lie Theory Cheat Sheet |
https://github.com/artivis/manif/blob/devel/paper/Lie_theory_cheat_sheet.pdf |
一页纸速查所有Lie group的exp/log/adj/Jacobian公式 |
★★★★★ |
4. 工程应用案例
| 项目 |
链接 |
该专题知识的体现 |
| Sophus库 (strasdat/Sophus) |
https://github.com/strasdat/Sophus |
SLAM领域最广泛使用的李群C++库，ORB-SLAM/DSO均依赖。⭐2.4k |
| ORB-SLAM3 |
https://github.com/UZ-SLAMLab/ORB_SLAM3 |
内部用Sophus::SE3f管理位姿 |
| VINS-Mono |
https://github.com/HKUST-Aerial-Robotics/VINS-Mono |
四元数+Eigen实现SO(3)，pose_local_parameterization.cpp展示Ceres流形参数化 |
| Pinocchio刚体动力学 |
https://github.com/stack-of-tasks/pinocchio |
基于Lie群实现RNEA/CRBA/ABA及解析导数，机械臂正/逆动力学核心 |
5. 直觉化类比与推荐学习路径
三层直觉：① 物理直觉——SO(3)是所有旋转的集合，像一个3D实心球（直径π），对径点粘合（转180°和-180°是同一个旋转）。李代数so(3)是贴在单位元处的切平面，就是角速度向量生活的空间。② 几何直觉——exp映射是"沿切线方向绕回球面"：你在切平面上画一个向量（角速度×时间），exp就把它卷回旋转群上对应的旋转。③ 工程直觉——旋转矩阵有9个元素但只有3个自由度，直接对9个元素求导会炸；而李代数只有3个分量，在这个空间里做微积分=无约束优化，做完再exp映射回去。这就是SLAM优化器需要李群的全部原因。
推荐路径：① Myron Phan博客建立"圆=李群"直觉 → ② 3Blue1Brown + Ben Eater四元数交互 → ③ 知乎"为啥需要李群"建立SLAM动机 → ④ Solà Micro Lie Theory精读 + YouTube讲座 → ⑤ 高翔十四讲第4讲跑Sophus代码 → ⑥ ORB-SLAM3/VINS-Mono代码阅读。
6. 中文社区优质资源
| 名称 |
链接 |
类型 |
推荐 |
| 知乎：李群和李代数——名字听起来很猛其实也没那么复杂 |
https://zhuanlan.zhihu.com/p/358455662 |
文章(1700+赞) |
★★★★★ |
| 知乎：视觉SLAM学习之李群李代数 |
https://zhuanlan.zhihu.com/p/388778282 |
文章 |
★★★★ |
| Sophus库API速查手册 (zxl19) |
https://zxl19.github.io/sophus-note/ |
博客 |
★★★★★ |
| CSDN：Sophus库安装和使用教程 |
https://blog.csdn.net/u011092188/article/details/77833022 |
博客 |
★★★★ |
| 《视觉SLAM十四讲》配套代码 |
https://github.com/gaoxiang12/slambook2 |
GitHub |
★★★★★ |
专题4：雅可比矩阵体系与BCH公式
1. 核心可视化资源
| 名称 |
链接 |
类型 |
核心价值 |
推荐 |
| Ceres官方：Automatic Derivatives图解 |
http://ceres-solver.org/automatic_derivatives.html |
官方文档 |
Jet/对偶数原理，AutoDiff vs NumericDiff vs AnalyticDiff性能对比图 |
★★★★★ |
| MIT 18.S096：Forward & Reverse Mode AD讲义 |
https://ocw.mit.edu/courses/18-s096-matrix-calculus-for-machine-learning-and-beyond-january-iap-2023/mit18_s096iap23_lec08.pdf |
讲义 |
含计算图可视化、forward/reverse mode对比 |
★★★★★ |
| GTSAM by Example: IMU Preintegration 101 |
https://gtbook.github.io/gtsam-examples/ImuFactorExample101.html |
Notebook |
Python完整示例：配置→ImuFactor→LM优化→轨迹绘图 |
★★★★★ |
2. 必看视频清单
| 名称 |
链接 |
核心价值 |
推荐 |
| Cyrill Stachniss — Bundle Adjustment Basics (Part 1) |
https://www.youtube.com/watch?v=sobyKHwgB0Y |
Jacobian结构、稀疏性、Schur补的**直觉化讲解** |
★★★★★ |
| Cyrill Stachniss — BA Numerics (Part 2) |
https://www.youtube.com/watch?v=LKDLcKrWOIU |
LM算法、Hessian稀疏结构 |
★★★★★ |
| Frank Dellaert: BA Tutorial (CVPR14) |
https://www.cs.cmu.edu/~kaess/vslam_cvpr14/media/VSLAM-Tutorial-CVPR14-A13-BundleAdjustment-handout.pdf |
SE(3)上Taylor展开、因子图formulation |
★★★★★ |
3. 优秀博客与文章
| 名称 |
链接 |
核心价值 |
推荐 |
| Telesens: BA Part 1 — Jacobians |
https://www.telesens.co/2016/10/13/bundle-adjustment-part-1-jacobians/ |
最佳BA Jacobian手推教程：从相机参数到投影方程逐步推导 |
★★★★★ |
| Tangram Vision: IMU Preintegration Basics (Part 5) |
https://www.tangramvision.com/blog/imu-preintegration-basics-part-5-of-5 |
高质量IMU系列最终篇，直觉化讲解预积分概念和bias Jacobian意义 |
★★★★★ |
| Forster TRO 2016 + 补充材料 |
https://rpg.ifi.uzh.ch/docs/TRO16_forster.pdf / https://rpg.ifi.uzh.ch/docs/RSS15_Forster_Supplementary.pdf |
最核心论文+所有Jacobian完整推导 |
★★★★★ |
| Rooniq: AD using Dual Numbers (CV视角) |
https://www.rooniq.com/blog/automatic-differentiation-using-dual-numbers |
从CV优化出发讲Jet代数，C++实现教程 |
★★★★★ |
| GTSAM Factor Graph Tutorial |
https://gtsam.org/tutorials/intro.html |
Dellaert权威教程，因子图理论+代码 |
★★★★★ |
4. 工程应用案例
| 项目 |
链接 |
该专题知识的体现 |
| GTSAM (borglab) |
https://github.com/borglab/gtsam |
Forster预积分论文的**官方配套实现**，含ImuFactor和CombinedImuFactor |
| Ceres Solver |
https://github.com/ceres-solver/ceres-solver |
Google开源非线性最小二乘，SLAM/BA标准工具，autodiff.h展示Jet模板元编程 |
| VINS-Mono integration_base.h |
https://github.com/HKUST-Aerial-Robotics/VINS-Mono |
核心代码：预积分midPointIntegration + 15维残差Jacobian |
| 崔华坤 VIO-Doc |
https://github.com/StevenCui/VIO-Doc |
VINS/MSCKF/ROVIO的**中文公式推导PDF**，被大量中文博客引用 |
5. 直觉化类比与推荐学习路径
BCH公式的直觉：两个李代数元素X和Y对应两个旋转，"先转X再转Y"的结果用李代数怎么表达？如果李群是交换的（如平移群），答案就是X+Y；但旋转不交换，所以结果是X+Y+½[X,Y]+...，Lie bracket [X,Y]精确量化了"不交换的程度"。在SLAM中，BCH公式的一阶近似告诉我们：在当前估计附近施加小扰动δ，新的李代数值≈旧值+J·δ，这个**J就是左/右Jacobian**——它是BCH公式的副产品。
预积分Jacobian的直觉：IMU预积分把两个关键帧之间的所有IMU测量"压缩"成一个相对运动量。当优化器调整bias估计时，不需要重新积分——只需用Jacobian做一阶修正：Δ_corrected ≈ Δ_old + J_bias · δbias。这个J_bias就是"bias变化多少会影响预积分结果多少"的灵敏度矩阵。
推荐路径：① Solà Micro Lie Theory理解BCH在SLAM中的角色 → ② 知乎"详解IMU预积分"五步推导法 → ③ Forster论文+补充材料精读 → ④ Tangram Vision博客建立直觉 → ⑤ GTSAM IMU Example跑代码 → ⑥ Telesens BA Jacobian手推 → ⑦ Ceres AutoDiff文档理解对偶数。
6. 中文社区优质资源
| 名称 |
链接 |
类型 |
推荐 |
| 知乎：详解IMU预积分——VINS中的预积分 |
https://zhuanlan.zhihu.com/p/534577566 |
文章 |
★★★★★ |
| 知乎：白话VINS-Mono之IMU预积分（一） |
https://zhuanlan.zhihu.com/p/407892357 |
文章 |
★★★★★ |
| 知乎：VINS-Mono代码详细解读3—预积分残差Jacobian协方差 |
https://zhuanlan.zhihu.com/p/148229464 |
文章 |
★★★★★ |
| 博客园：IMU预积分F矩阵逐项Jacobian推导 |
https://www.cnblogs.com/weihao-ysgs/p/IMU-Pre-Integration.html |
博客 |
★★★★★ |
| 崔华坤 VIO-Doc (中文PDF推导) |
https://github.com/StevenCui/VIO-Doc |
GitHub |
★★★★★ |
专题5：李群上的不确定性与概率分布
1. 核心可视化资源
| 名称 |
链接 |
类型 |
核心价值 |
推荐 |
| Banana Distribution代码 (Chirikjian Lab) |
https://github.com/ChirikjianLab/banana_distribution |
MATLAB代码 |
差分驱动机器人在SE(2)上的不确定性：笛卡尔下呈"香蕉形"，指数坐标下呈高斯分布，复现论文全部图 |
★★★★★ |
| Brossard SE₂(3)预积分协方差代码 |
https://github.com/mbrossar/SE2-3- |
Python+GTSAM |
IMU预积分协方差传播对比图、偏差更新图，PyTorch Monte-Carlo采样 |
★★★★★ |
| UKF-M多教程Notebooks |
https://github.com/CAOR-MINES-ParisTech/ukfm |
Python+MATLAB |
2D定位、3D IMU姿态、惯性导航、**KITTI IMU-GNSS融合**等多个可运行notebook |
★★★★★ |
2. 必看视频清单
| 名称 |
链接 |
核心价值 |
推荐 |
| Joan Solà — Lie Theory for the Roboticist |
https://youtu.be/QR1p0Rabuww |
从复数和单位圆直觉出发，涵盖协方差传播和EKF on Lie groups |
★★★★★ |
| InEKF-Localization项目演示 |
https://team16-mobrob-w20.github.io/inekf-localization/ |
在NCLT数据集上**可视化对比EKF vs Left-InEKF**的轨迹精度 |
★★★★ |
| Chirikjian — Stochastic Methods for Robotics (Georgia Tech) |
https://mediaspace.gatech.edu/media/Lecture+6A+Stochastic+Methods+for+Robotics+-+Gregory+S.+Chirikjian/1_njb3ev6d |
李群上随机过程、Fokker-Planck方程 |
★★★★ |
3. 优秀博客与文章
| 名称 |
链接 |
核心价值 |
推荐 |
| Tim Barfoot《State Estimation for Robotics》第二版(免费PDF) |
https://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf |
机器人状态估计权威教材，矩阵李群方法处理旋转不确定性 |
★★★★★ |
| Barrau & Bonnabel — Invariant Kalman Filtering 综述 |
https://arxiv.org/pdf/1410.1465 |
InEKF最全面综述：方法论+几何直觉+收敛保证+工业应用(无人机导航)+SLAM一致性 |
★★★★★ |
| Banana Distribution论文 (RSS 2012) |
https://www.roboticsproceedings.org/rss08/p34.pdf |
图示3和4直观展示"为什么要在李群上做概率" |
★★★★★ |
4. 工程应用案例
| 项目 |
链接 |
该专题知识的体现 |
| kalmanif (artivis) |
https://github.com/artivis/kalmanif |
基于manif的EKF/IEKF/UKF-M/RTS平滑器，直接对比不同滤波器 |
| RossHartley/invariant-ekf |
https://github.com/RossHartley/invariant-ekf |
InEKF C++实现+ROS包，支持足式机器人关节编码器 |
| Contact-Aided InEKF (UMich BipedLab) |
https://github.com/UMich-BipedLab/Contact-Aided-Invariant-EKF |
足式机器人Cassie的**接触辅助InEKF**工程化部署 |
| OpenVINS |
https://github.com/rpng/open_vins |
基于MSCKF的完整VIO系统，EuRoC/TUM-VI多数据集支持 |
5. 直觉化类比与推荐学习路径
Banana Distribution的直觉：想象一辆差分驱动小车从原点出发，有噪声地往前走再拐弯。在笛卡尔坐标下画不确定性，会得到一根弯弯的"香蕉"——因为角度误差会把位置误差"甩"到弧线上。但如果用李代数坐标（平移+旋转的局部坐标），同样的不确定性就是一个漂亮的椭球高斯分布。这就是"在李群上做概率"的核心收益：弯曲空间中的分布在切空间里变得规整。
InEKF的直觉：传统EKF在当前估计点线性化，如果估计偏了，线性化就不准，协方差就不可信——这叫"不一致性"。InEKF的妙处在于：对某类系统（群仿射系统），误差传递矩阵与状态估计无关，所以即使估计偏了，协方差照样准确。这不是近似，而是精确的数学性质——相当于在弯曲流形上找到了一个"天然的线性化坐标系"。
推荐路径：① Banana Distribution论文+代码建立可视化直觉 → ② Solà讲座理解协方差传播 → ③ Barfoot教材系统学习 → ④ UKF-M notebooks动手跑KITTI → ⑤ Barrau综述理解InEKF → ⑥ kalmanif库对比EKF/IEKF/UKF。
6. 中文社区优质资源
| 名称 |
链接 |
类型 |
推荐 |
| 赵宇(Jerry Zhao)：不变扩展卡尔曼滤波系列(3篇) |
https://aipiano.github.io/2019/03/23/不变扩展卡尔曼滤波1/ |
博客 |
★★★★★ |
| 知乎：VINS-Mono代码详细解读3—Jacobian协方差 |
https://zhuanlan.zhihu.com/p/148229464 |
文章 |
★★★★★ |
| CSDN：ORB-SLAM3中IMU预积分过程 |
https://blog.csdn.net/qq_39266065/article/details/115703313 |
博客 |
★★★★ |
| 知乎：IKFOM流形上迭代卡尔曼滤波 |
https://zhuanlan.zhihu.com/p/649097264 |
文章 |
★★★ |
专题6：等变理论与几何前沿
1. 核心可视化资源
| 名称 |
链接 |
类型 |
核心价值 |
推荐 |
| e3nn Tutorial Notebooks (Tess Smidt) |
https://blondegeek.github.io/e3nn_tutorial/ |
Jupyter交互 |
几何张量转换、等变网络的3个直觉任务、卷积设置逐步演练，Plotly 3D可视化 |
★★★★★ |
| 球谐函数交互可视化 (irhum) |
https://irhum.github.io/blog/spherical-harmonics/index.html |
交互博客 |
从傅里叶→球谐的直觉推导，大量交互3D图，与e3nn的联系讲得清楚 |
★★★★★ |
| E(2)-Steerable CNN动画 (QUVA-Lab) |
https://github.com/QUVA-Lab/e2cnn |
PyTorch库+动画 |
README含旋转等变性的直观动画：输入旋转→等变特征场变化验证 |
★★★★★ |
| TEASER++点云配准演示 |
https://github.com/MIT-SPARK/TEASER-plusplus |
C++/Python库 |
能容忍**99%外点**的鲁棒点云配准，含Open3D可视化pipeline |
★★★★★ |
2. 必看视频清单
| 名称 |
链接 |
核心价值 |
推荐 |
| AMMI Geometric Deep Learning完整课程 (2022) |
https://www.youtube.com/playlist?list=PLn2-dEmQeTfSLXW8yXP4q_Ii58wFdxb3C |
Bronstein/Bruna/Cohen/Veličković四位亲授12讲+Tutorial Colab |
★★★★★ |
| ArduPilot Conference — EqF VIO Presentation |
https://pvangoor.github.io/talks/2021/03/17/ardupilot_vio.html |
Mahony团队讲解等变滤波器VIO理论+真实户外飞行数据 |
★★★★★ |
| ML Street Talk — Geometric Deep Learning |
https://www.youtube.com/watch?v=bIZB1hIJ4u8 |
对话式通俗入门几何深度学习 |
★★★★ |
3. 优秀博客与文章
| 名称 |
链接 |
核心价值 |
推荐 |
| Geometric Deep Learning Proto-Book |
https://geometricdeeplearning.com/ |
几何深度学习圣经，从Erlangen纲领统一CNNs/GNNs/Transformers |
★★★★★ |
| Bronstein ICLR 2021 Keynote Blog |
https://medium.com/data-science/geometric-foundations-of-deep-learning-94cdd45b451d |
Klein Erlangen纲领→深度学习统一框架的精彩叙述 |
★★★★★ |
| Maurice Weiler — Equivariant CNNs博客系列 |
https://maurice-weiler.gitlab.io/blog_post/cnn-book_1_equivariant_networks/ |
等变CNN最权威直觉化数学解释，精美动画连接理论与escnn库 |
★★★★★ |
| Fuchs & Wagstaff — CNNs and Equivariance |
https://fabianfuchsml.github.io/equivariance1of2/ |
直觉化拆解G-CNN，"Picasso类比"讲为何中间层不能太早不变 |
★★★★★ |
| SE(3)-Equivariant Robot Learning教程综述 (2025) |
https://arxiv.org/html/2503.09829 |
面向机器人的等变深度学习+控制统一教程 |
★★★★★ |
4. 工程应用案例
| 项目 |
链接 |
该专题知识的体现 |
| Equivariant Diffusion Policy (CoRL 2024) |
https://equidiff.github.io/ + https://github.com/pointW/equidiff |
SO(2)等变改进扩散策略，MimicGen 12任务平均高baseline 21.9% |
| EqVIO (Mahony团队) |
https://github.com/pvangoor/eqvio |
等变滤波器VIO，EuRoC/UZH-FPV超越SOTA。IEEE TRO 2023 |
| SE-Sync |
https://github.com/david-m-rosen/SE-Sync |
可认证正确的pose-graph SLAM全局优化 |
| EquiBot (CoRL 2024) |
https://equi-bot.github.io/ |
SIM(3)等变+扩散模型，5分钟人类演示→真实机器人操作 |
| escnn库 (QUVA-Lab) |
https://github.com/QUVA-Lab/escnn |
2D/3D steerable CNN PyTorch扩展，可转换为纯PyTorch无额外开销 |
5. 直觉化类比与推荐学习路径
等变性的直觉：想象你在识别一只猫——不管猫在照片的左边还是右边，你都能认出来。这就是平移等变性，也是普通CNN自带的性质（通过权值共享/卷积实现）。现在问：如果猫旋转了90°呢？普通CNN就不保证了，必须靠数据增强"暴力记忆"各个角度。等变神经网络**则通过在网络结构中"编码"旋转对称性，让旋转后的输入自动产生对应旋转后的特征——不需要数据增强就能泛化。这就像物理定律不依赖你选什么坐标系一样：**网络的输出在对称变换下自洽。
球谐函数的直觉：就像傅里叶级数用正弦/余弦来分解1D信号，球谐函数用来分解球面上的信号。e3nn中的"irreps"(不可约表示)标记如"12x0e + 4x1o"，其中0e是标量（不随旋转变化），1o是向量（像箭头一样旋转），2e是应变张量（像椭球一样变形）——阶数越高，旋转下的变换规则越复杂。
推荐路径：① Fuchs博客建立等变直觉 → ② Bronstein ICLR博客获取统一框架 → ③ AMMI课程Lecture 3+8+Tutorial 2 → ④ e3nn Tutorial Notebooks动手 → ⑤ 球谐函数可视化理解irreps → ⑥ Weiler博客深入steerable理论 → ⑦ EqVIO/EquiDiff代码实践机器人应用。
6. 中文社区优质资源
| 名称 |
链接 |
类型 |
推荐 |
| 知乎(MindSpore)：等变神经网络与e3nn数学库 |
https://zhuanlan.zhihu.com/p/587704873 |
文章 |
★★★★★ |
| 知乎(MindSpore)：几何深度学习——统一视角下的神经网络 |
https://zhuanlan.zhihu.com/p/624124063 |
文章 |
★★★★★ |
| 知乎：Geometric Deep Learning二位作者三份视频报告 |
https://zhuanlan.zhihu.com/p/380893418 |
解读 |
★★★★★ |
| 集智俱乐部：从图神经网络到几何深度学习 |
https://swarma.org/?p=52106 |
科普 |
★★★★ |
| 知乎：谁来深入浅出剖析下Geometric Deep Learning？ |
https://www.zhihu.com/question/263633303 |
问答 |
★★★★ |
专题间交叉资源矩阵 ⭐
下表标注了每个资源对哪些专题有价值：
| 资源 |
专题1 |
专题2 |
专题3 |
专题4 |
专题5 |
专题6 |
| Sola "Micro Lie Theory" |
|
|
★★★ |
★★★ |
★★ |
|
| Barfoot "State Estimation" |
|
|
★★ |
★★★ |
★★★ |
★ |
| Boumal "Optimization on Manifolds" |
★ |
★★★ |
|
|
|
★ |
| Absil "Matrix Manifolds" |
★ |
★★★ |
|
|
|
|
| Lee "Smooth Manifolds" |
★★★ |
★ |
|
|
|
|
| Hall "Lie Groups" |
★ |
|
★★ |
★★ |
|
★★★ |
| Bronstein "Geometric DL" |
|
|
|
|
|
★★★ |
| Chirikjian "Stochastic Models" |
|
|
★ |
|
★★★ |
★ |
| Needham "Visual DG" |
★★★ |
★ |
|
|
|
|
| e3nn tutorials |
|
|
|
|
|
★★★ |
| Keenan Crane DDG |
★★★ |
★ |
|
|
|
|
| Forster "Preintegration" |
|
|
★ |
★★ |
★★★ |
|
| Rosen "SE-Sync" |
|
★★ |
|
|
|
★★★ |
| Geomstats library |
★ |
★★ |
★★ |
|
★ |
|
开源库完整指南 ⭐⭐
C++ 库（机器人工程首选）
| 库名 |
链接 |
覆盖专题 |
核心特点 |
推荐场景 |
| manif |
github.com/artivis/manif |
3-5 |
Header-only, 模板化, 解析 Jacobian |
理解李群操作的最佳学习代码 |
| Sophus |
github.com/strasdat/Sophus |
3-4 |
轻量, Eigen 集成, Ceres 兼容 |
快速原型, ORB-SLAM 系列 |
| GTSAM |
github.com/borglab/gtsam |
2-5 |
因子图, incremental, Shonan |
完整 SLAM/VIO 后端 |
| Pinocchio |
github.com/stack-of-tasks/pinocchio |
3-4 |
刚体动力学, 解析导数 |
运动控制与规划 |
| Eigen::Geometry |
eigen.tuxfamily.org |
3 |
四元数, 旋转矩阵, 齐次变换 |
基础几何运算 |
Python 库（研究与验证）
| 库名 |
链接 |
覆盖专题 |
核心特点 |
推荐场景 |
| Geomstats |
github.com/geomstats/geomstats |
1-5 |
最全面的流形计算库, GPU 支持 |
流形统计, 学习, 可视化 |
| Pymanopt |
github.com/pymanopt/pymanopt |
2 |
CG/trust-region/SD on manifolds |
中小规模精确流形优化 |
| Geoopt |
github.com/geoopt/geoopt |
2 |
PyTorch 流形优化器 |
深度学习中的流形约束 |
| e3nn |
github.com/e3nn/e3nn |
6 |
E(3) 等变神经网络 |
等变模型构建 |
| ESCNN |
github.com/QUVA-Lab/escnn |
6 |
可转steerable CNN |
图像/3D 等变处理 |
| SciPy Rotation |
scipy.spatial.transform |
3 |
旋转表示互转, Slerp |
快速验证与可视化 |
| SymPy |
sympy.org |
1-4 |
符号计算 |
公式验证, 推导检查 |
| allantools |
github.com/aewallin/allantools |
5 |
Allan 方差计算 |
IMU 噪声标定 |
Julia 库
| 库名 |
链接 |
覆盖专题 |
核心特点 |
| Manifolds.jl |
juliamanifolds.github.io |
1-2 |
retraction 种类最全, 文档出色 |
| Rotations.jl |
github.com/JuliaGeometry/Rotations.jl |
3 |
所有旋转表示互转 |
| LieGroups.jl |
github.com/JuliaManifolds/LieGroups.jl |
3-4 |
李群操作与 Jacobian |
SymPy/SageMath 验证食谱 ⭐⭐
食谱1：验证 Rodrigues 公式
import sympy as sp
theta = sp.Symbol('theta', positive=True)
n1, n2, n3 = sp.symbols('n1 n2 n3')
# 构造单位向量条件
N = sp.Matrix([[0, -n3, n2], [n3, 0, -n1], [-n2, n1, 0]])
# Rodrigues 公式
R = sp.eye(3) + sp.sin(theta)*N + (1-sp.cos(theta))*N**2
# 验证 R^T R = I (利用 n1^2+n2^2+n3^2=1)
RtR = sp.simplify(R.T * R)
# 需要手动 subs n1^2+n2^2+n3^2 = 1
RtR_simplified = RtR.subs(n1**2 + n2**2 + n3**2, 1)
print("R^T R - I =", sp.simplify(RtR_simplified - sp.eye(3)))
食谱2：验证 BCH 二阶项
import sympy as sp
from sympy import Matrix, symbols, Rational
# SO(3) 上验证 BCH 二阶
a1, a2, a3 = symbols('a1 a2 a3')
b1, b2, b3 = symbols('b1 b2 b3')
def hat(v):
return Matrix([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
A = hat([a1, a2, a3])
B = hat([b1, b2, b3])
# [A, B] = AB - BA
comm = A*B - B*A
# 验证 [A,B] 仍是反对称的
print("反对称?", sp.simplify(comm + comm.T) == sp.zeros(3))
# 提取 vee([A,B]) = a × b
# comm 的 vee 应该等于叉乘
cross = Matrix([a2*b3 - a3*b2, a3*b1 - a1*b3, a1*b2 - a2*b1])
print("Lie bracket = 叉乘?",
sp.simplify(Matrix([comm[2,1], comm[0,2], comm[1,0]]) - cross) == sp.zeros(3, 1))
食谱3：验证 Adjoint 公式
import numpy as np
from scipy.spatial.transform import Rotation
from scipy.linalg import expm
# 数值验证 Ad_{Exp(xi)} = exp(ad_xi)
# 对 SO(3): Ad_R(omega) = R @ omega
phi = np.array([0.3, -0.5, 0.7])
# 方法1：直接计算 Ad
R = Rotation.from_rotvec(phi).as_matrix()
omega_test = np.array([1.0, 0.0, 0.0])
Ad_result = R @ omega_test
# 方法2：exp(ad_phi) @ omega
# ad_phi = [phi]_x
ad_phi = np.array([[0, -phi[2], phi[1]],
[phi[2], 0, -phi[0]],
[-phi[1], phi[0], 0]])
exp_ad = expm(ad_phi)
exp_ad_result = exp_ad @ omega_test
print("两种方法一致?", np.allclose(Ad_result, exp_ad_result))
# 应输出 True
食谱4：验证左/右 Jacobian 互转
import numpy as np
def so3_left_jacobian(phi):
theta = np.linalg.norm(phi)
if theta < 1e-10:
return np.eye(3)
n = phi / theta
N = np.array([[0, -n[2], n[1]], [n[2], 0, -n[0]], [-n[1], n[0], 0]])
return (np.sin(theta)/theta)*np.eye(3) + \
(1 - np.sin(theta)/theta)*np.outer(n, n) + \
((1-np.cos(theta))/theta)*N
# 验证 J_l(phi) = Ad(Exp(phi)) * J_r(phi)
phi = np.array([0.4, -0.2, 0.6])
Jl = so3_left_jacobian(phi)
Jr = so3_left_jacobian(-phi) # J_r(phi) = J_l(-phi)
R = Rotation.from_rotvec(phi).as_matrix()
# Ad_{Exp(phi)} 在 SO(3) 上就是 R 本身
print("J_l = Ad * J_r?", np.allclose(Jl, R @ Jr))
食谱5：Monte Carlo 验证协方差传播
import numpy as np
from scipy.spatial.transform import Rotation
def sample_SO3_gaussian(R_mean, sigma, n=10000):
"""在 SO(3) 上采样 Concentrated Gaussian"""
xi_samples = np.random.randn(n, 3) * sigma
R_samples = [R_mean @ Rotation.from_rotvec(xi).as_matrix()
for xi in xi_samples]
return R_samples, xi_samples
# 验证复合公式
sigma1, sigma2 = 0.05, 0.03
Sigma1 = sigma1**2 * np.eye(3)
Sigma2 = sigma2**2 * np.eye(3)
R1_mean = Rotation.from_rotvec([0.5, 0, 0]).as_matrix()
R2_mean = Rotation.from_rotvec([0, 0.3, 0]).as_matrix()
# 解析公式
Ad_R2_inv = R2_mean.T # Ad(R^{-1}) = R^T for SO(3)
Sigma_12_analytic = Ad_R2_inv @ Sigma1 @ Ad_R2_inv.T + Sigma2
# Monte Carlo
n_mc = 50000
xi1 = np.random.multivariate_normal(np.zeros(3), Sigma1, n_mc)
xi2 = np.random.multivariate_normal(np.zeros(3), Sigma2, n_mc)
R12_mean = R1_mean @ R2_mean
xi12_samples = []
for i in range(n_mc):
R1 = R1_mean @ Rotation.from_rotvec(xi1[i]).as_matrix()
R2 = R2_mean @ Rotation.from_rotvec(xi2[i]).as_matrix()
R12 = R1 @ R2
xi12 = Rotation.from_matrix(R12_mean.T @ R12).as_rotvec()
xi12_samples.append(xi12)
Sigma_12_mc = np.cov(np.array(xi12_samples).T)
print("解析-MC 差异:", np.linalg.norm(Sigma_12_analytic - Sigma_12_mc))
常见公式速查表 ⭐⭐
SO(3) 核心公式
| 公式名 |
表达式 |
条件 |
| Rodrigues Exp |
\(e^{\theta n^\wedge} = I + \sin\theta\,n^\wedge + (1-\cos\theta)(n^\wedge)^2\) |
\(\|n\|=1\) |
| Log 映射 |
\(\theta = \arccos\frac{\operatorname{tr}R-1}{2}\), \(\phi = \frac{\theta}{2\sin\theta}(R-R^\top)^\vee\) |
\(R \ne -I\) |
| 左 Jacobian |
\(J_l = \frac{\sin\theta}{\theta}I + (1-\frac{\sin\theta}{\theta})nn^\top + \frac{1-\cos\theta}{\theta}n^\wedge\) |
\(\theta = \|\phi\|\) |
| 右 Jacobian |
\(J_r(\phi) = J_l(-\phi)\) |
|
| \(J_l^{-1}\) |
\(I - \frac{1}{2}\phi^\wedge + (1/\theta^2 - (1+\cos\theta)/(2\theta\sin\theta))(\phi^\wedge)^2\) |
\(\theta \ne 0, 2\pi\) |
| Adjoint |
\(\operatorname{Ad}_R\omega = R\omega\) |
|
| ad 算子 |
\(\operatorname{ad}_\phi\psi = \phi\times\psi = [\phi]_\times\psi\) |
|
| BCH 二阶 |
\(\log(\exp\phi\exp\psi)^\vee \approx \phi+\psi+\frac{1}{2}\phi\times\psi\) |
\(\|\phi\|+\|\psi\| \ll 1\) |
SE(3) 核心公式（平移在前 \(\xi=[\rho,\phi]^\top\)）
| 公式名 |
表达式 |
备注 |
| hat 映射 |
\(\xi^\wedge = \begin{pmatrix}\phi^\wedge & \rho \\ 0 & 0\end{pmatrix}\) |
\(4\times4\) |
| Exp 映射 |
\(\begin{pmatrix}e^{\phi^\wedge} & V\rho \\ 0 & 1\end{pmatrix}\) |
\(V\) 含三角函数 |
| \(V\) 矩阵 |
\(I + \frac{1-\cos\theta}{\theta^2}\phi^\wedge + \frac{\theta-\sin\theta}{\theta^3}(\phi^\wedge)^2\) |
|
| 逆元 |
\(T^{-1} = \begin{pmatrix}R^\top & -R^\top t \\ 0 & 1\end{pmatrix}\) |
|
| Adjoint |
\(\operatorname{Ad}_T = \begin{pmatrix}R & [t]_\times R \\ 0 & R\end{pmatrix}\) |
\(6\times6\) |
| 协方差复合 |
\(\Sigma_{12} = \operatorname{Ad}(\bar X_2^{-1})\Sigma_1\operatorname{Ad}(\bar X_2^{-1})^\top + \Sigma_2\) |
右扰动 |
数值安全阈值
| 情况 |
推荐阈值 |
处理方式 |
| Exp 小角度 |
\(\theta < 10^{-8}\) |
Taylor: \(\sin\theta/\theta \approx 1 - \theta^2/6\) |
| Exp 近 \(\pi\) |
\(\theta > \pi - 10^{-6}\) |
特征值分解或四元数路径 |
| Log 小角度 |
\(\theta < 10^{-8}\) |
Taylor: \(\theta/(2\sin\theta) \approx 1/2 + \theta^2/12\) |
| \(J_l^{-1}\) 小角度 |
\(\theta < 10^{-8}\) |
\(J_l^{-1} \approx I - \frac{1}{2}\phi^\wedge + \frac{1}{12}(\phi^\wedge)^2\) |
| 有限差分步长 |
\(\epsilon = 10^{-7}\) |
中心差分 \((f(x+\epsilon)-f(x-\epsilon))/(2\epsilon)\) |
Convention 快速对照表 ⭐⭐
切向量排序
| 库/论文 |
SE(3) 排序 |
四元数顺序 |
扰动方向 |
| Sola (manif) |
\([\rho, \phi]\) 平移前 |
Hamilton \((w,x,y,z)\) |
右扰动 |
| Barfoot |
\([\phi, \rho]\) 旋转前 |
— |
混用，明确标注 |
| GTSAM |
\([\phi, \rho]\) 旋转前 |
Hamilton |
右扰动 (ChartAtOrigin) |
| Sophus |
\([\rho, \phi]\) 平移前 |
Hamilton |
右扰动 |
| Ceres |
用户自定义 |
Hamilton |
通过 Manifold 接口 |
| OpenVINS |
\([\phi, \rho]\) 旋转前 |
JPL \((x,y,z,w)\) |
右扰动 |
| ORB-SLAM3 |
— |
Hamilton |
右扰动 |
| Pinocchio |
\([v, \omega]\) 线性前 |
Hamilton |
取决于接口 |
Hamilton vs JPL 四元数
| 性质 |
Hamilton |
JPL |
| 存储顺序 |
\((w,x,y,z)\) |
\((x,y,z,w)\) |
| \(q\) 旋转向量 \(v\) |
\(v' = qvq^*\) |
\(v' = q^*vq\) |
| 旋转复合 |
\(q_{AB} = q_A q_B\) |
\(q_{AB} = q_B q_A\) |
| 微小旋转 |
\(\delta q \approx [1, \frac{1}{2}\delta\theta]\) |
\(\delta q \approx [\frac{1}{2}\delta\theta, 1]\) |
黄金法则：在一个项目中只使用一种约定。如果需要对接不同约定的库，在接口层做一次转换，并用单元测试锁定转换正确性。
完整代码模板库 ⭐⭐
模板1：SO(3) 完整操作类
"""SO(3) 李群操作完整模板 — 可作为学习和验证的参考实现"""
import numpy as np
from typing import Tuple
class SO3Ops:
"""SO(3) 李群核心操作"""
EPS = 1e-10 # 小角度阈值
@staticmethod
def hat(omega: np.ndarray) -> np.ndarray:
"""R^3 -> so(3): 向量到反对称矩阵"""
return np.array([[0, -omega[2], omega[1]],
[omega[2], 0, -omega[0]],
[-omega[1], omega[0], 0]])
@staticmethod
def vee(Omega: np.ndarray) -> np.ndarray:
"""so(3) -> R^3: 反对称矩阵到向量"""
return np.array([Omega[2,1], Omega[0,2], Omega[1,0]])
@staticmethod
def exp(phi: np.ndarray) -> np.ndarray:
"""指数映射: R^3 -> SO(3), Rodrigues 闭式"""
theta = np.linalg.norm(phi)
if theta < SO3Ops.EPS:
# Taylor: I + phi^ + (1/2)(phi^)^2
return np.eye(3) + SO3Ops.hat(phi) + 0.5 * SO3Ops.hat(phi) @ SO3Ops.hat(phi)
n = phi / theta
N = SO3Ops.hat(n)
return np.eye(3) + np.sin(theta)*N + (1-np.cos(theta))*(N@N)
@staticmethod
def log(R: np.ndarray) -> np.ndarray:
"""对数映射: SO(3) -> R^3"""
cos_theta = np.clip((np.trace(R) - 1) / 2, -1, 1)
theta = np.arccos(cos_theta)
if theta < SO3Ops.EPS:
return SO3Ops.vee(R - R.T) / 2
if abs(theta - np.pi) < SO3Ops.EPS:
# 接近 pi: 需要特殊处理
# 找 R 的最大对角元素对应的列
diag = np.diag(R)
k = np.argmax(diag)
v = R[:, k] + np.eye(3)[:, k]
v = v / np.linalg.norm(v)
return v * theta
return theta / (2 * np.sin(theta)) * SO3Ops.vee(R - R.T)
@staticmethod
def left_jacobian(phi: np.ndarray) -> np.ndarray:
"""SO(3) 左 Jacobian 闭式"""
theta = np.linalg.norm(phi)
if theta < SO3Ops.EPS:
return np.eye(3) + 0.5 * SO3Ops.hat(phi)
n = phi / theta
N = SO3Ops.hat(n)
return (np.sin(theta)/theta)*np.eye(3) + \
(1 - np.sin(theta)/theta)*np.outer(n,n) + \
((1-np.cos(theta))/theta)*N
@staticmethod
def right_jacobian(phi: np.ndarray) -> np.ndarray:
"""SO(3) 右 Jacobian: J_r(phi) = J_l(-phi)"""
return SO3Ops.left_jacobian(-phi)
@staticmethod
def adjoint(R: np.ndarray) -> np.ndarray:
"""Ad_R: SO(3) 的 Adjoint 就是旋转矩阵本身"""
return R.copy()
@staticmethod
def verify_SO3(R: np.ndarray, tol: float = 1e-8) -> bool:
"""验证矩阵是否属于 SO(3)"""
return (np.linalg.norm(R.T@R - np.eye(3)) < tol and
abs(np.linalg.det(R) - 1) < tol)
模板2：SE(3) 核心操作
"""SE(3) 李群操作模板 (平移在前排序: xi = [rho, phi])"""
import numpy as np
class SE3Ops:
"""SE(3) 核心操作"""
@staticmethod
def exp(xi: np.ndarray) -> np.ndarray:
"""指数映射: R^6 -> SE(3), 返回 4x4 齐次矩阵"""
rho, phi = xi[:3], xi[3:]
theta = np.linalg.norm(phi)
R = SO3Ops.exp(phi)
if theta < 1e-10:
V = np.eye(3) + 0.5 * SO3Ops.hat(phi)
else:
N = SO3Ops.hat(phi/theta)
V = np.eye(3) + ((1-np.cos(theta))/theta**2)*SO3Ops.hat(phi) + \
((theta-np.sin(theta))/theta**3)*(SO3Ops.hat(phi)@SO3Ops.hat(phi))
T = np.eye(4)
T[:3,:3] = R
T[:3,3] = V @ rho
return T
@staticmethod
def log(T: np.ndarray) -> np.ndarray:
"""对数映射: SE(3) -> R^6"""
R = T[:3,:3]
t = T[:3,3]
phi = SO3Ops.log(R)
theta = np.linalg.norm(phi)
if theta < 1e-10:
V_inv = np.eye(3) - 0.5 * SO3Ops.hat(phi)
else:
N = SO3Ops.hat(phi/theta)
half_theta = theta / 2
V_inv = np.eye(3) - 0.5*SO3Ops.hat(phi) + \
(1/theta**2)*(1 - theta*np.cos(half_theta)/(2*np.sin(half_theta))) * \
(SO3Ops.hat(phi)@SO3Ops.hat(phi))
rho = V_inv @ t
return np.concatenate([rho, phi])
@staticmethod
def adjoint(T: np.ndarray) -> np.ndarray:
"""6x6 Adjoint 矩阵 (平移在前排序)"""
R = T[:3,:3]
t = T[:3,3]
Ad = np.zeros((6,6))
Ad[:3,:3] = R
Ad[:3,3:] = SO3Ops.hat(t) @ R
Ad[3:,3:] = R
return Ad
@staticmethod
def inverse(T: np.ndarray) -> np.ndarray:
"""SE(3) 逆元"""
R = T[:3,:3]
t = T[:3,3]
T_inv = np.eye(4)
T_inv[:3,:3] = R.T
T_inv[:3,3] = -R.T @ t
return T_inv
模板3：有限差分 Jacobian 验证器
def numerical_jacobian_SO3(func, R, eps=1e-7):
"""
对 SO(3) 上的函数 func(R) 计算数值右 Jacobian
func: SO(3) -> R^m
返回 m x 3 的 Jacobian
"""
f0 = func(R)
m = len(f0) if hasattr(f0, '__len__') else 1
J = np.zeros((m, 3))
for i in range(3):
delta = np.zeros(3)
delta[i] = eps
# 右扰动: R * Exp(delta)
R_plus = R @ SO3Ops.exp(delta)
R_minus = R @ SO3Ops.exp(-delta)
J[:, i] = (func(R_plus) - func(R_minus)) / (2*eps)
return J
def verify_jacobian(J_analytic, J_numeric, name="", tol=1e-5):
"""比较解析 Jacobian 与数值 Jacobian"""
err = np.linalg.norm(J_analytic - J_numeric) / max(np.linalg.norm(J_numeric), 1e-10)
status = "PASS" if err < tol else "FAIL"
print(f"[{status}] {name}: 相对误差 = {err:.2e}")
return err < tol
模板4：协方差传播 Monte Carlo 验证
def mc_verify_compounding(R1_mean, Sigma1, R2_mean, Sigma2,
n_samples=100000):
"""
Monte Carlo 验证 SO(3) 协方差复合公式
"""
from scipy.spatial.transform import Rotation
# 解析公式
Ad_R2_inv = R2_mean.T # SO(3) 的 Ad(R^{-1}) = R^T
Sigma_12_analytic = Ad_R2_inv @ Sigma1 @ Ad_R2_inv.T + Sigma2
# Monte Carlo
xi1 = np.random.multivariate_normal(np.zeros(3), Sigma1, n_samples)
xi2 = np.random.multivariate_normal(np.zeros(3), Sigma2, n_samples)
R12_mean = R1_mean @ R2_mean
xi12_list = []
for i in range(n_samples):
R1 = R1_mean @ SO3Ops.exp(xi1[i])
R2 = R2_mean @ SO3Ops.exp(xi2[i])
R12 = R1 @ R2
xi12 = SO3Ops.log(R12_mean.T @ R12)
xi12_list.append(xi12)
xi12_arr = np.array(xi12_list)
Sigma_12_mc = np.cov(xi12_arr.T)
rel_err = np.linalg.norm(Sigma_12_analytic - Sigma_12_mc) / \
np.linalg.norm(Sigma_12_mc)
print(f"协方差传播验证: 相对误差 = {rel_err:.4f}")
print(f" (误差 < 0.05 说明一阶近似在此协方差量级下有效)")
return rel_err
调试检查清单 ⭐⭐
当涉及李群的代码出现问题时，按以下顺序排查：
Level 1：基本正确性
- [ ] Exp(Log(X)) == X 对各种角度成立？（小角度、正常、接近 pi）
- [ ] Log(Exp(xi)) == xi 对各种大小的 xi 成立？
- [ ] 旋转矩阵满足 \(R^\top R = I\) 和 \(\det R = 1\)？
- [ ] SE(3) 矩阵的最后一行是 [0,0,0,1]？
Level 2：Convention 一致性
- [ ] 切向量排序：\([\rho, \phi]\) 还是 \([\phi, \rho]\)？与所用库一致？
- [ ] 扰动方向：左扰动还是右扰动？全代码统一？
- [ ] 四元数：Hamilton 还是 JPL？与所用库一致？
- [ ] Adjoint 方向：搬运时用 \(\operatorname{Ad}(X)\) 还是 \(\operatorname{Ad}(X^{-1})\)？
Level 3：数值稳定性
- [ ] 小角度分支是否正确触发？（\(\theta < 10^{-8}\) 时用 Taylor）
- [ ] 接近 \(\pi\) 的旋转是否有特殊处理？
- [ ] 有限差分步长是否合适？（\(10^{-7}\) 通常最优）
Level 4：Jacobian 验证
- [ ] 解析 Jacobian 与数值有限差分一致？（相对误差 < \(10^{-5}\)）
- [ ] 在边界条件处（\(\theta = 0\), \(\theta \to \pi\)）也通过？
- [ ] Jacobian 的维数正确？（残差维度 x 状态维度）
Level 5：不确定性传播
- [ ] 协方差正定？（最小特征值 > 0）
- [ ] NEES 在合理范围内？（\(\chi^2\) 分布的置信区间）
- [ ] Monte Carlo 验证与解析公式一致？
六专题知识依赖图 ⭐
专题1：光滑流形 ──────→ 专题2：Retraction ──→ 流形优化
│ │
│ ↓
↓ GTSAM/Ceres/SE-Sync
专题3：李群 SO3/SE3 ──→ 专题4：Jacobian/BCH ──→ SLAM后端
│ │
│ ↓
↓ IMU预积分/VIO
专题5：李群不确定性 ──→ 专题6：等变理论 ──→ 等变DL/InEKF
│
↓
ESKF/MEKF/UKF-M
每个箭头表示"必须先学完上游才能进入下游"。横向箭头表示紧密相关但可以部分并行。
学术论文阅读指南 ⭐⭐
入门级论文（适合第一遍接触李群机器人学）
| 论文 |
年份 |
页数 |
核心贡献 |
阅读优先级 |
| Sola et al. "A micro Lie theory" |
2018 |
55 |
机器人李群速查手册 |
★★★★★ 必读 |
| Eade "Lie Groups for CV" |
2017 |
20 |
极简工程入门 |
★★★★ |
| Forster et al. "On-Manifold Preintegration" |
2017 |
16 |
IMU 预积分标准 |
★★★★★ 必读 |
| Barfoot & Furgale "Associating Uncertainty" |
2014 |
17 |
协方差传播基础 |
★★★★ |
进阶论文（深入某个方向时阅读）
| 论文 |
年份 |
方向 |
核心贡献 |
| Rosen et al. "SE-Sync" |
2019 |
可认证SLAM |
SDP 松弛 + 全局最优证书 |
| Barrau & Bonnabel "InEKF" |
2017 |
等变滤波 |
不变误差 + 自治线性化 |
| Brossard et al. "UKF-M" |
2020 |
流形UKF |
sigma points on Lie groups |
| Boumal "Optimization on Manifolds" |
2023 |
流形优化 |
现代标准教材 |
| Thomas et al. "Tensor Field Networks" |
2018 |
等变网络 |
先驱工作 |
| Wang et al. "Equivariant Q-Learning" |
2021 |
等变RL |
开创性工作 |
论文阅读策略
对于涉及李群数学的论文，推荐以下阅读顺序：
- 先读 Section I (Introduction) + Section V/VI (Experiments)：了解动机和效果
- 再读 Section II (Preliminaries)：确认 convention（左/右扰动、切向量排序）
- 最后读 Section III-IV (Main contribution)：核心公式推导
特别注意：不同论文的 notation 可能不一致。建议在阅读前先建一个 convention 对照表：
- 该论文的 \(\oplus\) 操作对应哪种 retraction？
- 残差定义中 Log 的方向是什么？
- Jacobian 是关于左扰动还是右扰动？
学期课程规划模板 ⭐
16 周学期规划（每周约 8-10 小时）
| 周 |
内容 |
产出 |
检验标准 |
| 1-2 |
专题1：流形基础 |
理解切空间三种定义 |
能证明 SO(3) 切空间 |
| 3-4 |
专题2：Retraction |
实现球面 RGD |
Pymanopt 验证收敛 |
| 5-7 |
专题3：SO(3)/SE(3) |
实现 Exp/Log/Ad |
单元测试全通过 |
| 8-9 |
专题4：Jacobian/BCH |
手推 \(J_l\) 闭式 |
有限差分验证 |
| 10-11 |
专题5：不确定性 |
协方差传播验证 |
Monte Carlo 对比 |
| 12-14 |
专题6：等变理论 |
阅读 InEKF + e3nn 教程 |
简单等变网络训练 |
| 15-16 |
综合项目 |
选一个方向深入 |
能复现一篇论文结果 |
4 周速成规划（每周 20+ 小时，已有基础）
| 周 |
内容 |
重点 |
| 1 |
专题 1+2 速读 |
只看核心定义，不做习题 |
| 2 |
专题 3+4 精读 |
手推所有闭式，代码验证 |
| 3 |
专题 5 精读 |
协方差传播 + NEES |
| 4 |
专题 6 概览 + 项目 |
根据方向选择性深入 |
可视化工具使用指南 ⭐⭐
用 matplotlib 可视化 SO(3) 上的分布
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.spatial.transform import Rotation
def visualize_SO3_samples(R_mean, Sigma, n_samples=500):
"""可视化 SO(3) Concentrated Gaussian 采样
通过展示旋转后的单位向量在 S^2 上的分布"""
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(111, projection='3d')
# 参考方向
e3 = np.array([0, 0, 1])
# 绘制单位球面
u = np.linspace(0, 2*np.pi, 50)
v = np.linspace(0, np.pi, 30)
x = np.outer(np.cos(u), np.sin(v))
y = np.outer(np.sin(u), np.sin(v))
z = np.outer(np.ones_like(u), np.cos(v))
ax.plot_surface(x, y, z, alpha=0.1, color='gray')
# 均值方向
mean_dir = R_mean @ e3
ax.scatter(*mean_dir, color='red', s=100, label='Mean')
# 采样方向
for _ in range(n_samples):
xi = np.random.multivariate_normal(np.zeros(3), Sigma)
R = R_mean @ Rotation.from_rotvec(xi).as_matrix()
d = R @ e3
ax.scatter(*d, color='blue', alpha=0.1, s=5)
ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')
ax.legend()
plt.title('SO(3) Concentrated Gaussian on S^2')
plt.show()
# 使用示例
R_mean = Rotation.from_rotvec([0, 0, 0.5]).as_matrix()
Sigma = np.diag([0.01, 0.01, 0.05]) # 绕 z 轴不确定性大
# visualize_SO3_samples(R_mean, Sigma)
用 Geomstats 可视化测地线
# Geomstats 球面测地线可视化
import geomstats.backend as gs
import geomstats.visualization as visualization
from geomstats.geometry.hypersphere import Hypersphere
sphere = Hypersphere(dim=2)
# 两点间的测地线（大圆弧）
point_a = gs.array([1., 0., 0.])
point_b = gs.array([0., 0.7071, 0.7071])
# 生成测地线上的点
t = gs.linspace(0., 1., 50)
geodesic = sphere.metric.geodesic(point_a, point_b)
points_on_geodesic = geodesic(t)
# 可视化
fig = plt.figure(figsize=(8, 8))
ax = visualization.plot(points_on_geodesic, space='S2')
推荐学习路径（按目标方向） ⭐
方向A：SLAM/VIO 工程师
- 专题1 速读（1周）→ 专题3 精读（2周）→ 专题4 精读（2周）→ 专题5 §5.1-5.4（1周）
- 跳过专题2、6 的大部分，仅在需要时查阅
- 总计约 6-8 周
方向B：流形优化/可认证感知
- 专题1 精读（2周）→ 专题2 精读（3周）→ 专题3 速读（1周）→ 专题6 §6.5/§6.14（2周）
- 重点在专题2 的收敛性理论和 SDP 松弛
- 总计约 8-10 周
方向C：等变学习/几何深度学习
- 专题1 速读（1周）→ 专题3 §1-7（1.5周）→ 专题6 全部（4-6周）
- 重点在表示论和等变网络架构
- 总计约 7-9 周
方向D：全面覆盖（博士一年级推荐）
- 按顺序 1→2→3→4→5→6，每专题 2-3 周
- 总计约 12-18 周（一学期）
- 每周配合 SymPy 验证和代码实践
各专题核心定理清单（速查） ⭐⭐
专题1核心定理
| # |
定理 |
一句话陈述 |
机器人应用 |
| 1 |
反函数定理 |
切映射满秩 → 局部微分同胚 |
IK 局部可解性 |
| 2 |
正则值定理 |
约束 Jacobian 满秩 → 水平集是子流形 |
SO(3) 是子流形 |
| 3 |
Frobenius 定理 |
分布对合 ⟺ 完全可积 |
非完整约束分析 |
| 4 |
Stokes 定理 |
\(\int_M d\omega = \int_{\partial M}\omega\) |
保守力做功 |
| 5 |
Sard 定理 |
临界值集测度为零 |
几乎所有构型正则 |
专题2核心定理
| # |
定理 |
一句话陈述 |
工程含义 |
| 1 |
一阶 retraction 充分性 |
一阶条件保证 RGD 的一阶收敛 |
不需要精确 Exp |
| 2 |
Riemannian 梯度投影 |
grad f = Proj(nabla f) |
欧氏梯度投影即可 |
| 3 |
测地强凸收敛 |
线性收敛率 \((1-\mu/L)^k\) |
全局最优保证 |
| 4 |
Burer-Monteiro |
秩足够时无杂散极小 |
SE-Sync 全局最优 |
专题3核心公式
| # |
公式 |
表达式 |
使用场景 |
| 1 |
Rodrigues |
\(e^{\theta n^\wedge} = I + \sin\theta\,n^\wedge + (1-\cos\theta)(n^\wedge)^2\) |
旋转向量→矩阵 |
| 2 |
SE(3) Exp |
\((e^{\phi^\wedge}, V\rho)\) |
twist→位姿 |
| 3 |
逆元 |
\(T^{-1} = (R^\top, -R^\top t)\) |
坐标系互换 |
| 4 |
Adjoint |
\(\operatorname{Ad}_T = [R, t_\times R; 0, R]\) |
帧间搬运 |
专题4核心公式
| # |
公式 |
表达式 |
使用场景 |
| 1 |
\(J_l\) 定义 |
\(\int_0^1 e^{t\operatorname{ad}_\phi}dt\) |
Exp 的导数 |
| 2 |
\(J_r = J_l(-\phi)\) |
核心互转关系 |
左右扰动转换 |
| 3 |
BCH 一阶 |
\(\log(eXeY) \approx X+Y\) |
小扰动可加 |
| 4 |
BCH 二阶 |
\(+\frac{1}{2}[X,Y]\) |
非交换修正 |
专题5核心公式
| # |
公式 |
表达式 |
使用场景 |
| 1 |
Concentrated Gaussian |
\(X = \bar X\operatorname{Exp}(\xi),\;\xi\sim N(0,\Sigma)\) |
李群概率 |
| 2 |
复合 |
\(\Sigma_{12} = \operatorname{Ad}\Sigma_1\operatorname{Ad}^\top + \Sigma_2\) |
里程计累积 |
| 3 |
Adjoint 搬运 |
\(\Sigma_L = \operatorname{Ad}\Sigma_R\operatorname{Ad}^\top\) |
帧间转换 |
| 4 |
NEES |
\(\xi^\top\Sigma^{-1}\xi \sim \chi^2_n\) |
一致性检验 |
专题6核心概念
| # |
概念 |
定义要点 |
应用 |
| 1 |
等变映射 |
\(f(gx) = gf(x)\) |
InEKF, 等变网络 |
| 2 |
irrep |
最小不可分解表示 |
e3nn 特征类型 |
| 3 |
Schur 引理 |
交织映射 = 标量 or 零 |
等变层结构约束 |
| 4 |
CG 分解 |
\(\ell_1\otimes\ell_2 = \bigoplus D^L\) |
张量积层 |
典型工作流程图解 ⭐⭐
工作流1：SLAM 位姿图优化
输入：位姿图（节点 T_i ∈ SE(3)，边 Z_ij）
│
▼
Step 1: 定义残差 r_ij = Log(Z_ij^{-1} T_i^{-1} T_j) ← 专题3: Log 映射
│
▼
Step 2: 对 T_i 右扰动求 Jacobian J_i, J_j ← 专题4: J_r^{-1}, Ad
│
▼
Step 3: 组装法方程 J^T Σ^{-1} J δξ = -J^T Σ^{-1} r ← 专题5: 噪声模型
│
▼
Step 4: 求解 δξ (Cholesky/PCG)
│
▼
Step 5: 更新 T_i ← T_i · Exp(δξ_i) ← 专题2: Retraction
│
▼
收敛？── 否 → 回到 Step 1
│
是 → 输出优化后位姿
工作流2：VIO IMU 预积分
输入：IMU 测量序列 (ω_k, a_k), k = i..j
│
▼
Step 1: 递推旋转 ΔR_{k+1} = ΔR_k · Exp((ω_k - b_g)Δt) ← 专题3: SO(3) Exp
│
▼
Step 2: 递推速度 Δv_{k+1} = Δv_k + ΔR_k(a_k - b_a)Δt
│
▼
Step 3: 递推位置 Δp_{k+1} = Δp_k + Δv_k·Δt + ...
│
▼
Step 4: 递推协方差 Σ_{k+1} = A_k Σ_k A_k^T + B_k Q B_k^T ← 专题5: 传播
(A_k 包含 J_r) ← 专题4: J_r
│
▼
Step 5: bias 更新时，用 J_r 做一阶修正（不重新积分） ← 专题4: BCH
│
▼
输出：预积分量 (ΔR, Δv, Δp, Σ) 作为因子图的一个因子
工作流3：等变策略学习
输入：SE(3) 观测 (点云 + 末端位姿)
│
▼
Step 1: 将观测放入等变特征空间 ← 专题6: irrep 分解
- 位置: ℓ=1 向量
- 方向: ℓ=1 向量
- 距离: ℓ=0 标量
│
▼
Step 2: 等变消息传递 (e3nn tensor product) ← 专题6: CG 系数
│
▼
Step 3: 输出等变动作 ← 专题6: 等变映射
- SE(3) 位姿: 等变
- 夹爪开合: 不变
│
▼
验证：旋转输入 → 输出相应旋转？ ← 等变性检验
常见错误代码示例与修正 ⭐⭐
错误1：直接对旋转矩阵做加法更新
# ❌ 错误
R_new = R + dt * R_dot # 结果不在 SO(3) 上!
# 后果：R_new^T @ R_new != I，后续所有计算累积偏离
# ✅ 正确
omega = SO3Ops.vee(R.T @ R_dot) # 提取角速度
R_new = R @ SO3Ops.exp(omega * dt) # 指数映射更新
错误2：忽略切向量排序
# ❌ 错误：GTSAM 用 [phi, rho]，但你假设了 [rho, phi]
xi_gtsam = gtsam_result.xi() # [phi1, phi2, phi3, rho1, rho2, rho3]
rho = xi_gtsam[:3] # 错！这是 phi
phi = xi_gtsam[3:] # 错！这是 rho
# ✅ 正确：先确认排序，必要时转换
phi = xi_gtsam[:3] # GTSAM: 旋转在前
rho = xi_gtsam[3:] # GTSAM: 平移在后
# 如果你的代码用 [rho, phi] 排序：
xi_mine = np.concatenate([rho, phi]) # 显式转换
错误3：协方差搬运忘记 Adjoint
# ❌ 错误：直接加两个协方差
Sigma_12 = Sigma_1 + Sigma_2 # 忽略了坐标系差异
# ✅ 正确：用 Adjoint 搬运到同一切空间
Ad_X2_inv = SE3Ops.adjoint(SE3Ops.inverse(X2_mean))
Sigma_12 = Ad_X2_inv @ Sigma_1 @ Ad_X2_inv.T + Sigma_2
错误4：有限差分步长选择不当
# ❌ 错误：步长太大或太小
eps = 1e-3 # 太大：截断误差 O(eps^2) 不够小
eps = 1e-15 # 太小：浮点舍入误差主导
# ✅ 正确：eps ~ sqrt(machine_epsilon) ≈ 1e-7~1e-8
eps = 1e-7 # 对 double 精度最优
# 使用中心差分：(f(x+eps) - f(x-eps)) / (2*eps)
性能基准参考 ⭐
典型操作的计算时间（Intel i7, 单核, C++ -O2）：
| 操作 |
SO(3) |
SE(3) |
备注 |
| Exp (Rodrigues) |
~20 ns |
~50 ns |
含三角函数 |
| Exp (Cayley) |
~15 ns |
N/A |
仅矩阵操作 |
| Log |
~25 ns |
~60 ns |
含 arccos |
| 左 Jacobian |
~30 ns |
~80 ns |
闭式 |
| Adjoint |
~10 ns |
~20 ns |
矩阵组装 |
| 矩阵乘法 3x3 |
~5 ns |
N/A |
Eigen 优化 |
在 SLAM 后端中，如果有 10000 个位姿节点，每次迭代执行约 10000 次 Exp + 20000 次 Jacobian 计算，总计约 1-2 ms（C++ 优化实现）。这远小于求解法方程的时间（通常 10-100 ms），因此 Retraction 的选择在中等规模问题中不是性能瓶颈。
只有在超大规模（>100k 节点）或高频率（>100 Hz MPC）场景中，Cayley vs Exp 的选择才会有显著影响。
常见问题答疑 ⭐
Q1: 李群数学对做工程有什么直接帮助？
最直接的帮助是避免以下高频bug：
- 姿态优化后旋转矩阵不正交（加法更新 vs Exp 更新）
- 欧拉角奇异点导致滤波发散（坐标图奇异 vs 流形操作）
- 协方差椭球方向错误（忽略 Adjoint 搬运）
- IMU 预积分漂移（Jacobian 左右用错）
这些 bug 往往表现为"大部分时候正常、偶尔爆炸"，极难定位。理解底层数学是预防而非治疗。
Q2: 需要把所有证明都会推吗？
不需要。推荐策略是：
- 必须能手推的：Rodrigues 公式、SO(3) \(J_l\) 闭式、简单的协方差传播
- 理解思路即可的：BCH 高阶项、SE(3) 完整 Jacobian、Schur 引理证明
- 直接查表的：数值安全阈值、convention 对照、Allan 方差公式
Q3: 数学推导和代码实现哪个优先？
先理论后代码。但"先理论"不意味着"纯数学"——每推完一个公式，立刻用 SymPy/NumPy 验证一次。这种"推一步验一步"的节奏既能发现推导错误，又能建立公式与代码的直觉联系。
Q4: 推荐的第一本书是什么？
取决于方向：
- 做 SLAM：Sola "A micro Lie theory"（免费 arXiv 论文）
- 学流形优化：Boumal（免费在线教材 + 视频）
- 补数学基础：Tu "An Introduction to Manifolds"（含习题解答）
- 全面参考：Barfoot "State Estimation for Robotics" 2nd ed.
结语：从公式到直觉的最短路径
六个专题有一条共同的认知主线：弯曲空间里的问题，在切空间里做，做完映回去。流形的坐标卡是局部展平，retraction是优化步映回去，李群的exp映射是角速度变旋转，BCH/Jacobian是切空间里的链式法则，不确定性在切空间里近似高斯，等变网络把对称性编码进特征空间的"旋转规则"里。一旦抓住这条主线，六个专题的直觉就贯通了。
在资源选择上，有三个"超级节点"值得反复回看：Joan Sola的Micro Lie Theory（专题1-5的直觉枢纽）、Bronstein的Geometric Deep Learning Proto-Book（专题6的理论框架）、以及**Barfoot的State Estimation for Robotics**（专题3-5的工程教材）。这三份资料各自覆盖了一大片知识网络，从它们出发可以链接到几乎所有其他资源。
最终检验标准是：能否看到一个 SLAM/VIO/运动控制的代码库中某行涉及旋转/位姿的操作，立刻说出它背后的数学——"这里用了右扰动模型，对应 Exp 的右 Jacobian；那里的 Adjoint 是在把协方差从体帧搬到世界帧"。当你达到这个水平时，六个专题的知识就真正内化了。
最后，动手永远比看更高效。每个专题都提供了可直接运行的代码库——manif/Sophus用于李群编程、Pymanopt/Geoopt用于流形优化、GTSAM/Ceres用于因子图与自动微分、UKF-M/kalmanif用于滤波对比、e3nn/escnn用于等变网络。花一小时跑通一个example，胜过读十小时推导。
模板5：SE(3) BetweenFactor 残差与 Jacobian 验证 ⭐⭐
在因子图后端中，BetweenFactor 是最基本的位姿约束。它的残差定义为切空间中的向量，Jacobian 的正确性直接影响优化收敛。本模板提供残差计算和有限差分 Jacobian 验证的完整代码。
"""BetweenFactor 残差与 Jacobian 验证模板"""
import numpy as np
from scipy.spatial.transform import Rotation
def between_residual(Ti, Tj, Z_meas):
"""
计算 BetweenFactor 残差。
残差定义: r = Log(Z_meas^{-1} @ Ti^{-1} @ Tj)
Ti, Tj, Z_meas: 4x4 SE(3) 齐次矩阵
返回: 6 维切空间残差 [rho, phi]
"""
T_rel = se3_inv(Ti) @ Tj # 实际相对位姿
err_T = se3_inv(Z_meas) @ T_rel # 与测量的偏差
return se3_log(err_T) # 映射到切空间
def between_jacobian_numerical(Ti, Tj, Z_meas, eps=1e-7):
"""
数值计算 BetweenFactor 关于 Ti 和 Tj 的右 Jacobian。
返回: J_i (6x6), J_j (6x6)
"""
r0 = between_residual(Ti, Tj, Z_meas)
J_i = np.zeros((6, 6))
J_j = np.zeros((6, 6))
for k in range(6):
delta = np.zeros(6)
delta[k] = eps
# 对 Ti 的右扰动: Ti * Exp(delta)
Ti_plus = Ti @ se3_exp(delta)
Ti_minus = Ti @ se3_exp(-delta)
J_i[:, k] = (between_residual(Ti_plus, Tj, Z_meas)
- between_residual(Ti_minus, Tj, Z_meas)) / (2*eps)
# 对 Tj 的右扰动: Tj * Exp(delta)
Tj_plus = Tj @ se3_exp(delta)
Tj_minus = Tj @ se3_exp(-delta)
J_j[:, k] = (between_residual(Ti, Tj_plus, Z_meas)
- between_residual(Ti, Tj_minus, Z_meas)) / (2*eps)
return J_i, J_j
# 使用示例
Ti = se3_exp(np.array([1.0, 0.5, -0.3, 0.2, -0.1, 0.3]))
Tj = se3_exp(np.array([1.5, 0.8, 0.1, 0.3, 0.1, -0.2]))
Z_meas = se3_inv(Ti) @ Tj # 无噪声测量（完美情况下残差为零）
r = between_residual(Ti, Tj, Z_meas)
print(f"残差范数（应接近零）: {np.linalg.norm(r):.2e}")
# 加一点噪声验证 Jacobian
Z_noisy = Z_meas @ se3_exp(np.random.randn(6) * 0.01)
J_i_num, J_j_num = between_jacobian_numerical(Ti, Tj, Z_noisy)
print(f"J_i 条件数: {np.linalg.cond(J_i_num):.1f}")
print(f"J_j 条件数: {np.linalg.cond(J_j_num):.1f}")
验证原则：（1）无噪声测量时残差应精确为零（\(\|r\| < 10^{-14}\)）；（2）解析 Jacobian 与有限差分的逐元素相对误差应 \(< 10^{-5}\)；（3）在 \(\theta = 0\)（零旋转）和 \(\theta \to \pi\)（180 度旋转）附近都应通过验证。
模板6：NEES 一致性检验完整流程 ⭐⭐
NEES 检验是验证滤波器协方差估计是否合理的标准统计工具。本模板提供从 Monte Carlo 采样到置信带绘图的完整流程。
"""NEES 一致性检验完整模板"""
import numpy as np
from scipy.stats import chi2
import matplotlib.pyplot as plt
def run_nees_test(filter_func, true_dynamics, n_mc=100, n_steps=50,
state_dim=3, alpha=0.05):
"""
完整 NEES Monte Carlo 检验流程。
filter_func: 接收观测序列，返回 (estimates, covariances)
true_dynamics: 接收随机种子，返回 (true_states, observations)
n_mc: Monte Carlo 运行次数
n_steps: 时间步数
state_dim: 切空间维度（SO(3)=3, SE(3)=6）
"""
all_nees = np.zeros((n_mc, n_steps))
for i in range(n_mc):
# 生成真实轨迹和观测
true_states, observations = true_dynamics(seed=i)
# 运行滤波器
estimates, covariances = filter_func(observations)
for k in range(n_steps):
# 切空间误差（根据具体李群实现 Log 映射）
xi = compute_tangent_error(estimates[k], true_states[k])
P = covariances[k]
# NEES = xi^T P^{-1} xi
all_nees[i, k] = xi @ np.linalg.solve(P, xi)
# 计算平均 NEES 和置信区间
avg_nees = all_nees.mean(axis=0)
lower = chi2.ppf(alpha/2, n_mc * state_dim) / n_mc
upper = chi2.ppf(1 - alpha/2, n_mc * state_dim) / n_mc
# 绘图
fig, ax = plt.subplots(figsize=(12, 5))
ax.plot(avg_nees, 'b-', label='Average NEES')
ax.axhline(y=state_dim, color='g', linestyle='--', label=f'Expected ({state_dim})')
ax.fill_between(range(n_steps), lower, upper, alpha=0.2, color='green',
label=f'{int((1-alpha)*100)}% CI')
ax.set_xlabel('Time step')
ax.set_ylabel('Average NEES')
ax.legend()
ax.set_title(f'NEES Consistency Test (N={n_mc}, dim={state_dim})')
plt.tight_layout()
plt.savefig('nees_test.png', dpi=150)
plt.show()
# 一致性判断
n_outside = np.sum((avg_nees < lower) | (avg_nees > upper))
pct_outside = n_outside / n_steps * 100
print(f"超出置信区间的时刻: {n_outside}/{n_steps} ({pct_outside:.1f}%)")
print(f" (一致的滤波器应 < {alpha*100:.0f}%)")
return avg_nees, (lower, upper)
解读规则速查：
- 超出率 \(<5\%\)：滤波器一致
- 超出率 \(5\%\)-\(15\%\)：边界情况，需检查是否有系统性趋势
- 超出率 \(>15\%\)：滤波器不一致，需调整噪声参数或更换滤波器
模板7：Allan 方差快速计算与绘图 ⭐⭐
"""Allan 方差计算与绘图模板"""
import numpy as np
import matplotlib.pyplot as plt
def compute_allan_variance(data, dt, max_cluster_size=None):
"""
计算 Allan 方差。
data: 一维时间序列（静态 IMU 数据）
dt: 采样间隔（秒）
返回: taus（聚合时间）, adev（Allan 标准差）
"""
N = len(data)
if max_cluster_size is None:
max_cluster_size = N // 4
# 选择对数均匀分布的聚合大小
m_values = np.unique(np.logspace(0, np.log10(max_cluster_size),
100).astype(int))
m_values = m_values[m_values >= 1]
taus = []
adevs = []
for m in m_values:
tau = m * dt
# 非重叠 Allan 方差
n_clusters = N // m
if n_clusters < 2:
break
cluster_means = np.array([data[i*m:(i+1)*m].mean()
for i in range(n_clusters)])
avar = 0.5 * np.mean(np.diff(cluster_means)**2)
taus.append(tau)
adevs.append(np.sqrt(avar))
return np.array(taus), np.array(adevs)
def plot_allan_deviation(taus, adev, sensor_name="Gyro Z"):
"""绘制 Allan 偏差曲线并标注关键参数"""
fig, ax = plt.subplots(figsize=(10, 7))
ax.loglog(taus, adev, 'b-', linewidth=1.5)
# 标注斜率参考线
# ARW 斜率 -1/2
tau_arw = taus[taus < 1.0]
if len(tau_arw) > 2:
fit_mask = taus < 1.0
coeffs = np.polyfit(np.log10(taus[fit_mask]),
np.log10(adev[fit_mask]), 1)
arw_at_1s = 10**(coeffs[1]) # tau=1 处的截距
ax.loglog(tau_arw, arw_at_1s * tau_arw**(-0.5),
'r--', alpha=0.7, label=f'ARW slope (-1/2)')
ax.annotate(f'N = {arw_at_1s:.4f} deg/s/sqrt(Hz)',
xy=(1.0, arw_at_1s), fontsize=10, color='red')
# 零偏不稳定性（曲线最低点）
min_idx = np.argmin(adev)
bi = adev[min_idx] / 0.664 # IEEE 标准系数
ax.plot(taus[min_idx], adev[min_idx], 'go', markersize=10)
ax.annotate(f'BI = {bi:.4f} deg/h (tau={taus[min_idx]:.1f}s)',
xy=(taus[min_idx], adev[min_idx]),
xytext=(taus[min_idx]*5, adev[min_idx]*2),
arrowprops=dict(arrowstyle='->', color='green'),
fontsize=10, color='green')
ax.set_xlabel('Cluster Time tau (s)')
ax.set_ylabel('Allan Deviation')
ax.set_title(f'Allan Deviation - {sensor_name}')
ax.legend()
ax.grid(True, which='both', alpha=0.3)
plt.tight_layout()
plt.savefig(f'allan_deviation_{sensor_name.replace(" ", "_")}.png', dpi=150)
plt.show()
return arw_at_1s if len(tau_arw) > 2 else None, bi
# 使用示例（模拟数据）
# dt = 0.005 # 200 Hz
# gyro_data = np.cumsum(np.random.randn(200*3600) * 0.01) # 1h 静态数据
# taus, adev = compute_allan_variance(gyro_data, dt)
# arw, bi = plot_allan_deviation(taus, adev, "Gyro Z (simulated)")
跨专题快速参考卡 ⭐⭐
从问题到公式的速查表
当遇到工程问题时，按以下流程定位所需公式：
| 你要做什么 |
需要的公式 |
所在专题 |
关键参数 |
| 旋转向量 → 旋转矩阵 |
Rodrigues Exp |
专题3 |
\(\theta = \|\phi\|\) |
| 旋转矩阵 → 旋转向量 |
SO(3) Log |
专题3 |
注意 \(\theta \to \pi\) 奇异 |
| 两个位姿复合的协方差 |
\(\Sigma_{12} = \operatorname{Ad}\Sigma_1\operatorname{Ad}^\top + \Sigma_2\) |
专题5 |
右扰动，Adjoint 方向 |
| 验证滤波器一致性 |
NEES \(= \xi^\top\Sigma^{-1}\xi \sim \chi^2_n\) |
专题5 |
\(N \ge 50\) 次 MC |
| 在 SO(3) 上做优化 |
Retraction: \(R_{k+1} = R_k \operatorname{Exp}(\alpha \cdot \operatorname{grad})\) |
专题2 |
步长 \(\alpha\) 需 line search |
| 判断位姿图的全局最优性 |
SE-Sync SDP 松弛 dual gap |
专题6 |
gap \(< 10^{-6}\) → tight |
| 构建等变网络 |
irrep 类型 + CG 张量积 |
专题6 |
输出类型决定最后一层 |
| IMU 噪声标定 |
Allan 方差 → \(\sigma_g\) → \(Q_c = \sigma_g^2\) → \(Q_d = Q_c \Delta t\) |
专题5 |
单位转换！ |
| 预积分 bias 修正 |
\(\Delta_{\text{corr}} \approx \Delta + J_b \cdot \delta b\) |
专题4 |
\(J_b\) 是 bias Jacobian |
| 左/右协方差转换 |
\(\Sigma_L = \operatorname{Ad}(\bar X)\Sigma_R\operatorname{Ad}(\bar X)^\top\) |
专题5 |
确认 \(\bar X\) 还是 \(\bar X^{-1}\) |
从错误现象到原因的速查表
| 你遇到了什么问题 |
最可能的原因 |
第一步检查 |
相关专题 |
| 旋转矩阵 \(R^\top R \ne I\) |
在矩阵元素上做加法更新 |
改用 \(R \leftarrow R \operatorname{Exp}(\delta)\) |
专题3 |
| 优化后位姿"飞了" |
Jacobian 左右扰动用反 |
有限差分验证 Jacobian |
专题4 |
| 协方差方向明显错误 |
Adjoint 搬运方向 |
Monte Carlo 对比 |
专题5 |
| 长时间后 NEES 持续偏高 |
过程噪声 \(Q\) 太小 |
增大 \(Q\) 或分段预积分 |
专题5 |
| e3nn 输出对旋转不等变 |
irrep 类型声明错误 |
用随机旋转验证 \(f(Rx) \stackrel{?}{=} Rf(x)\) |
专题6 |
| GTSAM 与自实现结果不同 |
切向量排序不同 |
打印单位扰动后的位姿变化 |
附录 |
| 四元数均值"跳变" |
\(q/-q\) 双覆盖未处理 |
统一半球后再平均 |
专题5 |
| SE-Sync 报 "not tight" |
噪声过大或有外点 |
检查残差分布，加鲁棒核 |
专题6 |
| 有限差分 Jacobian 不准 |
步长太大或太小 |
用 \(\epsilon = 10^{-7}\)，中心差分 |
附录 |
| 协方差矩阵不正定 |
数值累积误差 |
Joseph 形式更新，对称化 |
专题5 |
库 API 速查：同一操作在不同库中的写法
| 操作 |
manif (C++) |
Sophus (C++) |
GTSAM (C++) |
scipy (Python) |
| Exp 映射 |
SO3d::exp(w) |
SO3d::exp(w) |
Rot3::Expmap(w) |
Rotation.from_rotvec(w) |
| Log 映射 |
R.log() |
R.log() |
Rot3::Logmap(R) |
R.as_rotvec() |
| 右扰动 |
R.rplus(delta) |
R * SO3d::exp(d) |
R.retract(d) |
R * Rotation.from_rotvec(d) |
| Adjoint |
R.adj() |
R.Adj() |
R.AdjointMap() |
手动 \(R\) (SO3) |
| 逆元 |
R.inverse() |
R.inverse() |
R.inverse() |
R.inv() |
| 复合 |
R1.compose(R2) |
R1 * R2 |
R1.compose(R2) |
R1 * R2 |
注意：GTSAM 的 retract 默认使用 ChartAtOrigin（右扰动），但某些旧版本可能使用不同的默认值。始终显式检查 convention。
调试工作流：当李群代码出 bug 时的系统排查策略 ⭐⭐
以下是一个经过验证的系统化排查流程。按层级从底向上排查，避免在高层浪费时间而底层有错。
============ Level 0: 基础运算 ============
[0.1] Exp(Log(X)) == X ?
→ 测试: X = identity, 小角度(0.01 rad), 中等(1 rad), 接近 pi(3.14 rad)
→ 失败说明: Exp 或 Log 实现有误
[0.2] Log(Exp(xi)) == xi ?
→ 测试: xi = 零向量, 小向量, 大向量(接近 pi)
→ 注意: 大角度时可能有分支选择问题
[0.3] Exp(xi) 在 SO(3) 上?
→ 检查: ||R^T R - I|| < 1e-12 AND |det(R) - 1| < 1e-12
→ 失败说明: Rodrigues 公式实现有误
============ Level 1: Convention 一致性 ============
[1.1] 切向量排序统一?
→ 方法: 对 SE(3)，施加 [1,0,0,0,0,0] 扰动，检查变化的是平移还是旋转
→ 若变化的是平移 → 排序为 [rho, phi]（manif/Sophus 风格）
→ 若变化的是旋转 → 排序为 [phi, rho]（GTSAM/OpenVINS 风格）
[1.2] 扰动方向统一?
→ 方法: 计算 X * Exp(delta) 和 Exp(delta) * X，看哪个与你的公式一致
→ 全代码必须统一右扰动或左扰动
[1.3] Adjoint 方向正确?
→ 方法: 验证 Exp(Ad_X xi) = X Exp(xi) X^{-1}
→ 用有限差分（非解析公式）做参照
============ Level 2: Jacobian 正确性 ============
[2.1] 解析 Jacobian vs 有限差分
→ 方法: 中心差分，eps = 1e-7
→ 标准: 逐元素相对误差 < 1e-5
→ 边界: 在 theta=0 和 theta->pi 都要测
[2.2] Jacobian 维度正确?
→ 残差维 m x 状态维 n，不要搞反
============ Level 3: 不确定性传播 ============
[3.1] 协方差正定?
→ 检查: 所有特征值 > 0
→ 失败说明: 公式有误或数值累积
[3.2] Monte Carlo 验证
→ 方法: N=100000 采样，比较解析与经验协方差
→ 标准: Frobenius 相对误差 < 0.05
[3.3] NEES 在合理范围?
→ 方法: N=100 次 MC，画平均 NEES 曲线
→ 标准: 95% 时刻在 chi^2 置信带内
============ Level 4: 系统集成 ============
[4.1] 多库混用时排序转换?
→ 方法: 显式用置换矩阵 P 转换，单元测试锁定
[4.2] 端到端验证
→ 方法: 简单问题（如匀速圆周运动）有解析解，
完整系统输出应与解析解一致
这个流程的设计原则是：每一层的正确性依赖于下一层。如果 Level 0 有 bug，在 Level 2 花再多时间调 Jacobian 也是白费。按层级向上排查是唯一高效的策略。