SLAM 如何一步步迭代到 SE(3)

从旋转的物理意义到旋转矩阵的数学性质再到刚体变换最后才到李群李代数欧拉角Euler Angle——最直观的旋转表示欧拉角 把任意 3D 旋转拆成先绕 Z 转、再绕 Y 转、再绕 X 转用三个角度来描述。三个角度有专用名字航空/无人机常用英文中文绕哪个轴想象成Yawψ​偏航角Z 轴飞机左右转头看左边/右边Pitchθ​俯仰角Y 轴飞机抬头/低头上看天/下看地Rollφ​滚转角X 轴飞机侧倾/翻滚向左歪/向右歪Z上 ↑ |__ Y右 / X前机头朝向你坐飞机时机长说的偏航、俯仰、滚转就是这三个。组合旋转SLAM 常用 Z→Y→X 顺序Yaw 30°, Pitch 15°, Roll 10°按顺序做三次旋转先绕自身 Z 轴转 30°Yaw再绕转完后的 Y 轴转 15°Pitch最后绕转完后的 X 轴转 10°Roll⚠️顺序不能乱​Yaw→Pitch→Roll 和 Roll→Pitch→Yaw 结果不一样绕各轴转角度 α 的基本旋转矩阵绕 XRollRx │ 1 0 0 │ │ 0 cosα -sinα │ │ 0 sinα cosα │绕 YPitchRy │ cosα 0 sinα │ │ 0 1 0 │ │ -sinα 0 cosα │绕 ZYawRz │ cosα -sinα 0 │ │ sinα cosα 0 │ │ 0 0 1 │完整旋转矩阵ZYX 顺序R Rz(Yaw) · Ry(Pitch) · Rx(Roll)→ 先算 Rx再 Ry最后 Rz左乘。欧拉角的两大缺点记住这俩❌ 1. 顺序依赖三组角度必须说清旋转顺序否则无意义。常用约定ZYX先 Yaw再 Pitch最后 Roll❌ 2. 万向锁Gimbal Lock——重点理解什么时候出事​当Pitch ±90°​ 时Roll 轴和 Yaw 轴重合三个自由度退化成两个。形象比喻——三环陀螺仪正常三个金属环互相垂直各自独立转 ✅Pitch 转 90° 后内环和外环在同一个平面 → 两个环锁在一起 ❌这时你转Roll和转Yaw效果一样丢了一个自由度旋转矩阵 RSO(3)——欧拉角之后自然出现的那个一、为什么欧拉角之后一定是它你刚才已经看到欧拉角有两个大坑顺序乱先转谁后转谁结果不一样万向锁Pitch 90° 时直接废一个自由度二、旋转矩阵一句话是什么旋转矩阵 把“旋转后的三个坐标轴”并排放在一起形象理解假设你站在房间里手里拿着一个坐标系红箭头X 轴绿箭头Y 轴蓝箭头Z 轴你转了一下身体旋转然后问自己三个问题原来的 X 轴现在指向哪儿原来的 Y 轴现在指向哪儿原来的 Z 轴现在指向哪儿把这三个答案写成列向量并排放在一起就是一个3×3 矩阵。三、第一个具体数字例子超重要例①什么都不转单位旋转你没动原来的轴还在原地原轴新方向X 轴(1, 0, 0)Y 轴(0, 1, 0)Z 轴(0, 0, 1)拼成矩阵R │ 1 0 0 │ │ 0 1 0 │ │ 0 0 1 │这就是单位矩阵 I。 含义没转原样。例②绕 Z 轴转 90°Yaw 90°你站在地上原地左转 90°。原轴转完之后指向X 轴原来 Y 方向 (0, 1, 0)Y 轴原来 -X 方向 (-1, 0, 0)Z 轴还是朝上 (0, 0, 1)拼成矩阵R_z90 │ 0 -1 0 │ │ 1 0 0 │ │ 0 0 1 │你可以这样验证自己原来朝前的 X 轴 → 现在朝左Y 正原来朝左的 Y 轴 → 现在朝后X 负✅ 完全符合直觉。例③欧拉角 vs 旋转矩阵对照理解欧拉角Yaw 90°, Pitch 0°, Roll 0°对应的旋转矩阵就是上面那个R_z90。 到这里你就明白一句话了**欧拉角是“怎么转的过程”旋转矩阵是“转完的结果”。**六、旋转矩阵的优缺点笔记重点优点缺点✅ 没有万向锁❌ 9 个数冗余✅ 旋转可以连乘❌ 不能加✅ 数学性质好❌ 不适合优化 所以 SLAM 里存位姿、算变换用旋转矩阵做优化、求导不用它四元数Quaternion——旋转世界的“稳定老司机”刚经历过欧拉角还记得它的两个致命伤吗顺序乱先转谁后转谁结果不一样万向锁Pitch 90° 直接废一个自由度于是数学家开始想有没有一种方式可以不用“分步转”没有奇点还能稳定插值、稳定计算答案就是四元数四元数 绕某根轴转某个角度就这么简单。它不是“三步转”而是一步到位往哪个方向转、转多少四元数最大的杀手锏插值SLAM / 动画最爱场景相机在时间 t₀ 的朝向相机在时间 t₁ 的朝向想知道中间某一帧的朝向用欧拉角❌ 会乱跳、穿模、万向锁用四元数✅球面线性插值SLERP路径最短速度均匀非常丝滑这就是为什么Unity / UnrealIMU / 无人机AR / VR全都在用四元数。四元数 vs 欧拉角对比记对比项欧拉角四元数直观✅ 非常❌ 一般万向锁❌ 有✅ 无插值❌ 差✅ 非常好计算稳定性❌ 一般✅ 很高参数数量34四元数在 SLAM 里干嘛用场景用不用给人看姿态❌转成欧拉角IMU 积分✅ 常用滤波EKF✅ 常用BA 优化❌用李代数一句话记住**四元数是“旋转的运动员”李代数是“优化的工具人”。**李代数 so(3) —— 旋转的“小增量 / 微调”一、为什么四元数之后一定是 so(3)你现在已经见过三种旋转表示表示特点欧拉角人看得懂但会炸旋转矩阵 R稳但不能加四元数稳但也不是向量现在问题来了一个非常工程化的问题如果我猜了一个位姿发现不太对我想“改一点点”怎么办比如相机往左挪 2 cm再往左转 0.5°这时候你就需要“旋转的小增量”这就是so(3)​ 登场的理由。二、一句话理解 so(3)so(3) 一个三维向量表示“往哪边转、转多少”它本质上就是✅轴角Axis-Angle三、so(3) 长什么样只看这个ϕ (ϕ₁, ϕ₂, ϕ₃)几何含义是分量含义ϕ₁绕 X 轴转多少ϕ₂绕 Y 轴转多少ϕ₃绕 Z 轴转多少 你可以把它当成“旋转速度的瞬时版本”四、第一个具体数字例子一定要看例①什么都不动ϕ (0, 0, 0) 对应没转单位旋转矩阵例②绕 Z 轴转 5°注意这里是小角度。ϕ (0, 0, 5°)如果你把它“用掉”就会得到一个接近单位矩阵的 RR ≈ │ cos5° -sin5° 0 │ │ sin5° cos5° 0 │ │ 0 0 1 │ 这就是 so(3) 的直觉它不是一个“完整旋转”而是“旋转的微分”例③相机往左歪一点点ϕ (0, 0, 0.5°)在 SLAM 里这种东西会出现在迭代优化梯度下降位姿微调五、为什么 so(3) 这么香✅ 1️⃣ 它是向量可以加假设你现在有两个小调整ϕ₁ (0, 0, 0.5°) ϕ₂ (0.2°, 0, 0)你可以直接加ϕ ϕ₁ ϕ₂ 这在旋转矩阵、四元数上是做不到的。✅ 2️⃣ 非常适合优化BA 的核心SLAM 在做的事情本质是不断给位姿加一个小增量 ϕ让误差变小流程是这样的当前位姿 T ↓ 算误差 ↓ 得到一个 se(3) / so(3) 增量 ξ ↓ T_new exp(ξ) · T_old ↓ 重复你不需要手算 exp只要知道优化 在李代数上加向量六、so(3) 和前面几个东西的关系重点东西层级欧拉角人用旋转矩阵 R最终结果四元数 q稳定表示so(3)优化用的增量一句话记住R / q 是“位置”so(3) 是“速度 / 微调”SE(3) —— 3D 世界里“相机/机器人的位姿”一、一句话是什么先记住这个SE(3) 旋转 平移打包成一个整体换句话说SE(3) 就是“相机在哪、朝哪看”的完整描述二、它是怎么被逼出来的① 旋转用 RSO(3)R ∈ SO(3)② 平移用 tt ∈ ℝ³最朴素的想法是(R, t)但马上出问题了问题原因怎么复合要先转再移顺序不能乱怎么求逆手算麻烦怎么统一运算不是一个东西 数学家就想能不能把 R 和 t 塞进同一个数学对象里三、齐次坐标关键转折点形象版普通坐标p (x, y, z)齐次坐标p_h (x, y, z, 1)多加一个 1看起来多余但它干了一件大事让“旋转 平移”变成一次矩阵乘法四、SE(3) 长什么样重点SE(3) 是一个4×4 矩阵T │ R t │ │ 0 1 │展开写出来就是T │ r11 r12 r13 tx │ │ r21 r22 r23 ty │ │ r31 r32 r33 tz │ │ 0 0 0 1 │部分含义左上 3×3旋转 RSO(3)右上 3×1平移 t最后一行固定 (0 0 0 1) 这就是SE(3)。五、第一个具体数字例子一定要看例①相机在原地没动R I t (0, 0, 0)对应的 SE(3)T │ 1 0 0 0 │ │ 0 1 0 0 │ │ 0 0 1 0 │ │ 0 0 0 1 │ 世界坐标系 相机坐标系。例②相机往前走了 1 米“前”是 Z 轴负方向SLAM 常用约定。R I t (0, 0, 1)T │ 1 0 0 0 │ │ 0 1 0 0 │ │ 0 0 1 1 │ │ 0 0 0 1 │ 相机在世界里沿 Z 轴走了 1 m。例③相机左转 90°再往前走旋转绕 Z 转 90°你之前见过的 R_z90平移t (0, 0, 1)T │ 0 -1 0 0 │ │ 1 0 0 0 │ │ 0 0 1 1 │ │ 0 0 0 1 │你可以这样读它这个相机左转 90°然后往前走 1 米一句话就描述完了 ✅六、SE(3) 在干的两件正经事✅ 1️⃣ 把一个点从相机坐标系变到世界坐标系p_world T · p_camera不用你管先转还是先移矩阵一次搞定。✅ 2️⃣ 把两个位姿拼起来比如先动一次 T₁再动一次 T₂T T₂ · T₁ 这就是 SLAM 里每天都在干的事。七、SE(3) 的“性格”优缺点优点缺点✅ 表达完整❌ 16 个数✅ 可复合❌ 不能加✅ 数学漂亮❌ 不适合优化 所以存位姿用 SE(3)优化用 se(3)se(3) —— 位姿的“微调旋钮”一、一句话是什么先记住se(3) 位姿的小增量 平移微调 旋转微调它是一个6 维向量ξ (ρ₁, ρ₂, ρ₃, ϕ₁, ϕ₂, ϕ₃)二、它从哪里来非常顺这条线SE(3) 位姿本身 ↓ 但 SE(3) 不能加 ↓ 那“改一点点位姿”怎么表示 ↓ se(3) 所以 se(3) 的存在理由只有一个为了优化位姿三、se(3) 各部分是什么意思ξ (ρ₁, ρ₂, ρ₃, ϕ₁, ϕ₂, ϕ₃)部分名字含义ρ平移增量往哪个方向挪多少ϕ旋转增量往哪边转多少 你可以直接把它当成“给当前相机拧的两个小旋钮”四、第一个具体数字例子一定要看例①完全不动ξ (0, 0, 0, 0, 0, 0) 对应单位变换矩阵。例②相机往左挪 2 cmξ (0, 0.02, 0, 0, 0, 0)前三个数平移后三个数旋转没有例③相机往左转 0.5°ξ (0, 0, 0, 0, 0, 0.5°) 这就是 so(3)现在嵌在 se(3) 里。例④真实 SLAM 里最常见的那种ξ (0.01, 0.005, 0, 0, 0, 0.3°)含义是往右挪 1 cm往前挪 0.5 cm往左转 0.3°✅直接加​✅不用管约束​✅不会坏五、se(3) 在 SLAM 里干嘛用✅ 核心用途BABundle AdjustmentBA 的本质是不断给每个位姿加一个小增量 ξ让误差变小流程是当前位姿 T ↓ 算误差 ↓ 求一个 se(3) 增量 ξ ↓ T_new exp(ξ) · T_old ↓ 重复 你永远不会直接改 R / t 你永远是改se(3)六、se(3) vs SE(3)对比记对比SE(3)se(3)是什么位姿位姿的增量形式4×4 矩阵6 维向量能不能加❌✅优化用❌✅约束很多几乎没有一句话记住SE(3) 是“答案”se(3) 是“步子” 笔记se(3) 里面的相关计算工程直觉版一、先给你一个“总览表”心里有底计算在干嘛向量 → 矩阵把 6 维向量变成 4×4 矩阵矩阵 → 向量把 4×4 矩阵变回 6 维向量指数映射 exp把“微调”变成“真实位姿”对数映射 log把“真实位姿”变成“微调”李括号 [·,·]描述“两次微调的差别”你写代码几乎只会用到 exp / log二、① 向量 → 矩阵∧ 运算输入se(3) 向量ξ (ρ₁, ρ₂, ρ₃, ϕ₁, ϕ₂, ϕ₃)输出4×4 矩阵ξ^ │ ϕ^ ρ │ │ 0 0 │具体数字例子ξ (1, 2, 3, 0, 0, 0.5)意思是平移(1, 2, 3)绕 Z 转 0.5 rad对应的矩阵ξ^ │ 0 -0.5 0 1 │ │ 0.5 0 0 2 │ │ 0 0 0 3 │ │ 0 0 0 0 │✅ 左上角是 so(3)✅ 右上角是平移✅ 最后一行全是 0 这一步只是为了数学结构完整你平时很少手写。三、② 矩阵 → 向量∨ 运算这是 ∧ 的逆操作。ξ^ → ξ 写代码时一般由库帮你做Sophus / Eigen。你只需要知道左上角 → 旋转向量右上角 → 平移向量四、③ 指数映射 exp最重要一句话是什么exp把“微调”变成“真实位姿”数学位置exp(ξ) T ∈ SE(3)具体数字例子经典输入纯平移ξ (1, 0, 0, 0, 0, 0)exp 后是T │ 1 0 0 1 │ │ 0 1 0 0 │ │ 0 0 1 0 │ │ 0 0 0 1 │ 平移 1 米没旋转。输入纯旋转ξ (0, 0, 0, 0, 0, 90°)exp 后是T │ 0 -1 0 0 │ │ 1 0 0 0 │ │ 0 0 1 0 │ │ 0 0 0 1 │ 左转 90°没平移。输入旋转 平移ξ (1, 0, 0, 0, 0, 90°)exp 后先转 90°再走 1 米 平移方向会被旋转“掰弯”。五、④ 对数映射 log第二重要一句话是什么log把“真实位姿”变成“微调”数学位置log(T) ξ ∈ se(3)什么时候用比如你有两个位姿 T₁、T₂想知道它们之间的“差值”做法是ξ log(T₂ · T₁⁻¹) 得到的就是从 T₁ 到 T₂ 的微调六、⑤ 李括号 [·,·]高级但要知道一句话是什么李括号 两次微调的顺序差直觉[ξ₁, ξ₂] ≈ 先 ξ₁ 再 ξ₂ − 先 ξ₂ 再 ξ₁具体数字感受ξ₁ (1, 0, 0, 0, 0, 0) ξ₂ (0, 1, 0, 0, 0, 0)先走 X 1 米再走 Y 1 米先走 Y 1 米再走 X 1 米 结果一样 → 李括号很小但如果加入旋转ξ₁ (0, 0, 0, 0, 0, 90°) ξ₂ (1, 0, 0, 0, 0, 0) 顺序不同结果完全不同 李括号 ≠ 0七、写代码时你真正会用到的重点CSophusSophus::SE3d T; // 优化变量 Eigen::Vector6d xi T.log(); // 更新位姿 T T * Sophus::SE3d::exp(xi);你永远不会手写 exp / log 公式本文系统介绍了3D旋转和位姿表示的核心概念欧拉角、旋转矩阵、四元数、李群SE(3)和李代数se(3)。欧拉角直观但存在顺序依赖和万向锁问题旋转矩阵描述结果但冗余四元数无奇点且插值稳定李代数so(3)/se(3)作为优化工具提供增量式调整。SE(3)整合旋转和平移而se(3)是其对应的微调空间。实际应用中欧拉角用于人机交互四元数用于稳定计算李代数用于优化迭代旋转矩阵和SE(3)用于存储最终位姿。这些表示各具特点在SLAM、机器人等领域形成互补使用模式。BA 的本质不是“优化位姿”而是“在 se(3) 上不断给位姿加微调”。 BA 的一次完整迭代故事线版我们把主角设定好一个相机一张图像一个 3D 点一个“猜得不太准”的初始位姿第 0 步初始状态世界长这样① 相机位姿SE(3)T 单位矩阵意思是相机在世界原点朝 Z 轴负方向看② 3D 点在世界上P_world (5, 0, 10)意思是这个点在前方 10 m偏右 5 m③ 投影到图像上理想情况相机看到这个点像素坐标应该是u 320 v 240第 1 步现实很骨感误差出现了但你实际在图像里检测到的像素是u_obs 310 v_obs 245 差了 10 个像素这说明一件事要么点不准要么相机位姿不准BA 的假设是点是对的位姿是错的第 2 步把误差“翻译”成语言你现在的问题是“相机要怎么动才能让投影误差变小”但相机位姿是 SE(3)你不能直接加。所以你必须✅先把 SE(3) 变成 se(3)第 3 步对数映射 log位姿 → 微调你做一件事ξ log(T)结果是ξ (0, 0, 0, 0, 0, 0) 因为你现在是单位位姿。第 4 步优化器算出“该怎么改”优化器比如高斯–牛顿算出一个增量Δξ (0.01, 0, 0, 0, 0, -0.3°)含义是相机往右挪 1 cm往左转 0.3°✅ 这是一个se(3)​✅ 它是一个向量✅ 可以直接加第 5 步更新微调向量加法ξ_new ξ Δξ这一步在旋转矩阵上做不到在四元数上也做不到只有在 se(3) 上能做到 这就是李代数存在的最大意义。第 6 步指数映射 exp微调 → 位姿你现在有了新的微调 ξ_new但你要用的是位姿 T。所以你做T_new exp(ξ_new)得到一个新的 SE(3) 位姿。第 7 步用新位姿重新投影你把同一个 3D 点用新的 T_new 再投一次u_proj 315 v_proj 243误差从10 px → 5 px 变小了 ✅第 8 步判断是否收敛误差还够小吗不够 → 回到第 4 步够了 → 结束