四个关键实测数字
| 指标 | 数值 |
|---|---|
| 7 条群 / 链式恒等式最大残差 | 8.9e−16 |
| 同一 (R,t)(R,t)(R,t) 换复合顺序的落点差 ∣(I−R)t∣|(I-R)t|∣(I−R)t∣ | 2.828 m |
| 0.5° 外参角误差在 10 m 处的落点误差 | 8.727 cm |
| 连乘 2000 步正交性漂移 → SVD 重正交化后 | 0.233 → 1.6e−16 |
0 · 导读:把「坐标系」这个词钉死
Day 2 把三个坐标系当成三张「带标签的格子纸」来用,这对入门够,但一旦要拼外参链、推雅可比、查 bug,就必须回答更基本的问题:坐标系是原点 + 一组基;坐标是向量在这组基上的系数;变换矩阵是「换基」而不是「移动点」。这三句决定了后面每一处符号。
一句话心智模型: 世界系 WWW、机体系 BBB、传感器系 CCC 是三套「原点 + 正交单位基」;TabT_{ab}Tab 把 b 系的坐标翻译成 a 系的坐标;SLAM 的状态是 Twb,kT_{wb,k}Twb,k 序列,地图点靠 TwbTbcT_{wb}T_{bc}TwbTbc 搬到世界系。全文只围绕这一条链展开。
本文的三条硬结论(全部由第 8 节脚本实跑验证)
- RRR 的列是基向量,不是「三个转角的查表结果」。 正交性 RTR=IR^\mathsf{T}R=IRTR=I 与 detR=+1\det R=+1detR=+1 都是「单位正交右手基」的直接后果;detR=−1\det R=-1detR=−1 的正交矩阵是镜像,不是旋转(H 组实测)。
- 齐次坐标不是记法糖,是唯一能把平移写成线性映射的办法。 3×3 线性映射必满足 f(0)=0f(0)=0f(0)=0,而平移 f(0)=t≠0f(0)=t\neq0f(0)=t=0;升到 w=1w=1w=1 超平面后平移变成 4D 里的剪切。补 w=0w=0w=0 则得到「不受平移影响的方向」。
- 坐标链里,旋转误差会被距离放大,平移误差不会。 外参角误差 δθ\delta\thetaδθ 让距离 ddd 处的点偏 2dsin(δθ/2)≈d δθ2d\sin(\delta\theta/2)\approx d\,\delta\theta2dsin(δθ/2)≈dδθ:0.5° 在 10 m 处就是 8.727 cm;而 1 cm 平移误差到哪儿都还是 1 cm。
与已有两篇的分工: Day 2 教程负责「跑通」;
slam-pose-trajectory-deep-dive负责「SO(3)/SE(3)、左乘右乘、漂移三定律」这些进阶内容;本文补的是它们之间被跳过的几何基础与严格推导,四元数、李代数、伴随只做指路不展开。
1 · 坐标系与坐标链:谁在翻译谁
这一节把「写下标」变成一件有物理含义的事。绝大多数外参 bug 不是算错,是下标含义没定死。
1.1 坐标系 = 原点 + 一组单位正交基
一个坐标系 aaa 由原点 OaO_aOa 与三个基向量 (ea,1,ea,2,ea,3)(e_{a,1},e_{a,2},e_{a,3})(ea,1,ea,2,ea,3) 组成,满足 ea,i⋅ea,j=δije_{a,i}\cdot e_{a,j}=\delta_{ij}ea,i⋅ea,j=δij,右手系下 e1×e2=e3e_1\times e_2=e_3e1×e2=e3。空间中的点是几何对象,坐标才是数字:
P=Oa+∑j=13pa,j ea,j,pa=[pa,1,pa,2,pa,3]T P = O_a + \sum_{j=1}^{3} p_{a,j}\,e_{a,j},\qquad p_a = [p_{a,1},p_{a,2},p_{a,3}]^\mathsf{T} P=Oa+j=1∑3pa,jea,j,pa=[pa,1,pa,2,pa,3]T
换一套基,同一个 PPP 的坐标变了,点本身没动。这就是「被动变换」(alias):换的是描述方式。
1.2 主动 vs 被动:同一个数值,两种相反的解释
| 主动(active / alibi) | 被动(passive / alias) | |
|---|---|---|
| 做了什么 | 点 / 刚体真的转了 θ\thetaθ,坐标系不动 | 点不动,坐标系转了 θ\thetaθ |
| 坐标怎么变 | p′=R(θ)pp'=R(\theta)pp′=R(θ)p | p′=R(θ)Tpp'=R(\theta)^\mathsf{T}pp′=R(θ)Tp |
| θ=30°\theta=30°θ=30° 时的 (1,0,0)(1,0,0)(1,0,0)(C 组实测) | (0.866, 0.500, 0) | (0.866, −0.500, 0) |
| SLAM 里对应 | 机器人真的转了个头 | 把「相机系里的点」翻译成「世界系里的坐标」 |
两者相差 RRR 与 RTR^\mathsf{T}RT,也就是转 +θ+\theta+θ 与 −θ-\theta−θ;C 组实测两者夹角 60° = 2×30°。关键提醒: pw=Rwbpb+twbp_w=R_{wb}p_b+t_{wb}pw=Rwbpb+twb 属于被动解释——机器人在那儿没动,我们只是把它的观测翻译成世界坐标。把它读成「主动把点转过去」,写逆变换时必错。
1.3 双下标约定与链式法则
约定(全文严格遵守): TabT_{ab}Tab 读作「把 b 系坐标变到 a 系坐标」,即 pa=Tab pbp_a=T_{ab}\,p_bpa=Tabpb。于是 tabt_{ab}tab 是「b 系原点在 a 系中的坐标」,而不是反过来。点一律用列向量,变换一律左乘。
pw=Twbpb=Twb(Tbcpc)=(TwbTbc)pc ⟹ Twc=TwbTbc p_w = T_{wb}p_b = T_{wb}(T_{bc}p_c) = (T_{wb}T_{bc})p_c \ \Longrightarrow\ \mathbf{T_{wc}=T_{wb}T_{bc}} pw=Twbpb=Twb(Tbcpc)=(TwbTbc)pc ⟹ Twc=TwbTbc
下标像「消元」一样接龙:w→b→cw\to b\to cw→b→c 中间的 bbb 抵消,得到 w→cw\to cw→c。由此还有两条天天用的关系:
Tba=Tab−1,Tbc=Twb−1Twc (相对位姿 = 左逆乘) T_{ba}=T_{ab}^{-1},\qquad T_{bc}=T_{wb}^{-1}T_{wc}\ \ (\text{相对位姿 = 左逆乘}) Tba=Tab−1,Tbc=Twb−1Twc (相对位姿 = 左逆乘)
A 组实测:链式与直乘残差 8.88e−16;Twb−1Twc−TbcT_{wb}^{-1}T_{wc}-T_{bc}Twb−1Twc−Tbc 的 Frobenius 范数 2.15e−16。浮点层面就是精确成立。
1.4 必须写进代码注释的约定
| 项目 | 本文约定 | 常见冲突 |
|---|---|---|
| 向量形态 | 列向量,p′=Tpp'=Tpp′=Tp | 行向量约定下是 p′=pTp'=pTp′=pT,整条链全部反过来 |
| 手性 | 右手系,e1×e2=e3e_1\times e_2=e_3e1×e2=e3 | 左手系会让 detR=−1\det R=-1detR=−1 混进来 |
| 机体系轴 | 前 X / 左 Y / 上 Z(ROS REP-103) | 相机系常用 右 X / 下 Y / 前 Z(OpenCV),两者差一个固定旋转 |
| 下标方向 | TabT_{ab}Tab:b → a | 部分库写 T_a_b 表示 a → b;跨库必查文档 |
| 单位 / 时间 | 米、弧度、TkT_kTk 属于时刻 k | ΔTk\Delta T_kΔTk 属于区间 [k, k+1],混用会差半帧 |
2 · 旋转 R:从基向量推出来,而不是背下来
Day 2 告诉你「R 是 3×3 正交矩阵,det=1」。这一节回答:这两个条件从哪来、少了会怎样、9 个数为什么只有 3 个自由度。
2.1 R 的第 j 列 = b 系第 j 根基向量在 a 系里的坐标
设 pb=[p1,p2,p3]Tp_b=[p_1,p_2,p_3]^\mathsf{T}pb=[p1,p2,p3]T,同一个几何向量(先忽略原点)在两套基下展开:
v=∑jpjeb,j,[v]a=∑jpj [eb,j]a v=\sum_j p_j e_{b,j},\qquad [v]_a=\sum_j p_j\,[e_{b,j}]_a v=<
447



被折叠的 条评论
为什么被折叠?



