本文从扫描匹配的 Hessian 结构出发,用公式解释两件事:
- 为什么 KITTI(远距离户外)场景的 Hessian 特征值/迹被撑大;
- 这一放大如何让退化检测误判,进而导致方案失效。
1. Hessian的迹
trace(H) = Σᵢ (‖pᵢ‖²·sin²φᵢ + 1),其中 ‖pᵢ‖ 是点到雷达的距离:
- 平移项
Σ 1只由点数决定,与距离无关; - 旋转项
Σ ‖pᵢ‖²·sin²φᵢ随距离的平方暴涨。
KITTI 的点在几十米外,‖pᵢ‖² 比室内大两个数量级,旋转项把 trace 撑到巨大;而退化检测看的 λ_min 落在平移方向、几乎不随距离涨,于是 λ_min / trace 被压小,阈值 trace·0.01 恒把健康帧误判成"退化"。
2. 迹的三个等价定义
对 6×6 的 Hessian H,"迹"(trace)是同一个数的三种读法:
Σλᵢ 特征值之和
= Σₖ Hₖₖ 对角线之和(迹的定义)
= Σᵢₖ Jᵢₖ² 所有雅可比元素的平方和(因为 H = JᵀJ)
其中:
- 迹 = 对角线之和:迹的定义就是把对角元加起来;
- 迹 = 雅可比平方和 :
H = JᵀJ,其对角元Hₖₖ = Σᵢ Jᵢₖ²(第 k 列的平方和),全加起来就是 J 所有元素平方和,也等于按行读的Σᵢ ‖Jᵢ‖²; - 迹 = 特征值之和 :
H实对称可对角化,而迹在相似变换下不变。
3. 完整推导
3.1 残差:点到平面的距离
LOAM/LIO-SAM 的 scan-to-map 优化,最小化点到对应平面的距离:
rᵢ = nᵢᵀ ( R·pᵢ + t − qᵢ )
符号:
| 符号 | 含义 |
|---|---|
pᵢ |
当前扫描点(雷达坐标系),‖pᵢ‖ = 点到雷达的距离 |
qᵢ |
地图中对应平面上的参考点 |
nᵢ |
平面单位法向量(‖nᵢ‖ = 1) |
R, t |
待优化的旋转、平移 |
3.2 雅可比:对 6 个自由度求导
优化变量是扰动 ξ = [δθ; δt](3 旋转 + 3 平移),小角度下 R ≈ I + [δθ]×。分别求导:
平移:∂rᵢ/∂δt = nᵢᵀ
旋转:∂rᵢ/∂δθ = (pᵢ × nᵢ)ᵀ
合起来,每个点贡献一行雅可比(1×6):
Jᵢ = [ (pᵢ × nᵢ)ᵀ , nᵢᵀ ]
└── 旋转 ──┘ └平移┘
3.3 Hessian 与迹
Gauss-Newton 的 Hessian(等权,加权情形完全一致):
H = Σᵢ Jᵢᵀ Jᵢ
它的迹等于所有点雅可比模长平方之和(见第 2 节):
trace(H) = Σᵢ ‖Jᵢ‖²
3.4 拆成"平移项 + 旋转项"
代入 Jᵢ:
‖Jᵢ‖² = ‖pᵢ × nᵢ‖² + ‖nᵢ‖²
由叉积模长 ‖pᵢ × nᵢ‖ = ‖pᵢ‖·‖nᵢ‖·sinφᵢ,且 ‖nᵢ‖ = 1:
‖Jᵢ‖² = ‖pᵢ‖²·sin²φᵢ + 1
其中 φᵢ 是 pᵢ 与法向量 nᵢ 的夹角。于是:
trace(H) = Σᵢ ( ‖pᵢ‖²·sin²φᵢ + 1 )
= Σᵢ ‖pᵢ‖²·sin²φᵢ + N
└────── 旋转项 ──────┘ └─平移项┘
N 为参与优化的点数。
3.5 距离以平方形式出现
对照两项,距离的存在一清二楚:
平移项 = N (每个点贡献 1,无距离)
旋转项 = Σᵢ ‖pᵢ‖²·sin²φᵢ (每个点贡献 距离² × 角度因子)
‖pᵢ‖²字面就是"点到雷达的距离,再平方";sin²φᵢ是 0~1 之间的角度因子,与距离无关。
结论:旋转项随距离平方增长,平移项完全不涨。
4. KITTI 与室内的量级对比
| 场景 | 典型 ‖pᵢ‖ | ‖pᵢ‖² |
|---|---|---|
| 室内 / 停车场 | ~3 m | 9 |
| KITTI 户外高速 | ~40 m(远景更远) | 1600 |
距离差 1 个数量级,‖pᵢ‖² 差 2 个数量级(约 180 倍)。
点数 N 两边相近(LOAM 每帧特征点有筛选上限),所以:
trace(KITTI) ≈ N·1600·⟨sin²φ⟩ + N
trace(室内) ≈ N·9·⟨sin²φ⟩ + N
旋转项把 KITTI 的 trace 顶高两个数量级------实测 trace ≈ 4.4×10⁵ 正是这个量级。
5. 六个特征值不是一起涨的
- 3 个平移特征值 ∝
N,不随距离变; - 3 个旋转特征值 ∝
N·‖p‖²,随距离平方暴涨。
因此 KITTI 下 Hessian 变得极端不对称:3 个旋转方向的特征值巨大,3 个平移方向的相对小,trace 实质被旋转方向主导。
6. 退化检测为什么因此失效
退化检测看的是最小特征值 λ_min ,它落在某个平移/弱平移方向,量级 ∝ N,几乎不随距离涨。
而自适应阈值 thresh = trace × 0.01 ∝ N·‖p‖²,被旋转项按距离平方撑大。于是比值:
λ_min / thresh ∝ N / (N·‖p‖²) = 1 / ‖p‖²
KITTI 距离大 → 该比值被压得极小 → λ_min < thresh 恒成立 → 每帧判"退化"。
后端随之把 LiDAR 因子协方差放大 1 + covar_scale_factor × factor ≈ 850×,LiDAR 约束在因子图中逐帧失效,轨迹退化为纯 IMU 推演而发散(RMSE 2.99 m → 630.9 m)。
7. 失效因果链(总结)
远处点
→ 旋转雅可比含 (pᵢ × nᵢ)
→ 模长含 ‖pᵢ‖²
→ 旋转特征值按距离平方暴涨
→ trace ∝ ‖p‖² 被撑大
→ 阈值 trace·0.01 虚高
→ λ_min(平移向)没涨,恒 < 阈值
→ 100% 帧误判退化
→ 后端把 LiDAR 协方差放大 ~850×
→ 轨迹发散
本质:退化应定义为"某方向约束相对其他方向弱"(尺度无关,如 λᵢ/λ_max 或旋转/平移子块各自的条件数),而 trace·ratio 把"整体尺度的距离平方放大"混进了判据,把健康场景误当成了退化。