李群小工具的数值卫生：归一化、夹角与阈值

1. 轨迹闭合了，姿态却在 jitter

预积分跑通、回环也触发，RViz 里轨迹却像「抖肩」——优化器报告收敛，field 里 yaw 在 π 邻域来回跳。根因常在 $S O (3)$ helper：指数映射小角近似用错区间、四元数未逐步归一化、误差在切空间与欧拉角混用。李群工具不是论文附录，是 ESKF 与 BA 的地基；写错一次，协方差更新与残差雅可比全在错流形上算，闭环后地图看似对齐，姿态却在控制环里持续振荡。这类 bug 单看日志往往只有「残差已小」，必须靠 round-trip 单测与 π 邻域样本才能抓住。

2. 归一化：每步 reproject，别等 NaN

积分 $q \leftarrow q \otimes exp (ω Δ t)$ 后必须归一化；范数漂移会让协方差更新失真，最后以 NaN 形式在优化器里爆炸。零范数 guard 防除零，但别用精确等于判单位四元数——单精度上永远 false，应近似容差或每次调用归一化。更稳的做法是周期性投影到单位球，而不是等范数漂到 $1 0^{- 4}$ 才补救。底盘上若只在初始化 normalize、积分链路上不 reproject，长轨迹后姿态误差会以「缓慢漂移」形式出现，难以和标定问题区分。

cpp

Eigen::Quaterniond integrateOmega(const Eigen::Quaterniond& q,
                                  const Eigen::Vector3d& w, double dt) {
  Eigen::Quaterniond dq(1, 0.5*w.x()*dt, 0.5*w.y()*dt, 0.5*w.z()*dt);
  return (q * dq).normalized();
}

3. Log/Exp 与 π 邻域

对数映射用 atan2 稳定形式，别用反余弦在 trace 接近三时数值爆炸。对比两姿态用角距离，误差 clamp 到 $(- π, π]$ 。小角 $sin θ \approx θ$ 仅 $θ < 1 0^{- 3}$ rad 可用；大角度仍用标准正弦——预积分里省几个 cycles 不够赔错误雅可比。旋转误差在 π 附近不连续，对比两个姿态前必须先统一半球。球面线性插值走最短弧；归一化线性插值快但非恒定角速度——控制环与可视化选型要一致，混用会导致角速度曲线不一致。

4. Tangent space 与协方差

协方差在 $so (3)$ 更新，别直接在欧拉角上加高斯噪声——盒加与盒减与 Exp/Log 成对出现，缺一半就会在错误空间做线性化。平面拟合前对高度做 robust scale；二十万点 naive 累加与补偿累加差几厘米，影响地面拟合与高度阈值。鲁棒核切换点后别在未加权残差上用近似相等判收敛，否则优化器会提前停在不一致点。

cpp

double sum = 0, c = 0;
for (double x : xs) {
  const double y = x - c;
  const double t = sum + y;
  c = (t - sum) - y;
  sum = t;
}

5. 插值与符号统一

四元数符号未统一会导致轨迹 flip——对比前统一半球（ $q_{w} \geq 0$ ）。插值全程检查范数接近一；点积接近负一时球面插值要走最短路径并 flip 符号，否则绕远路。可视化与控制若各用一套插值策略，回放轨迹与实际控制参考会对不上，field 表现为「RViz 里正常、车不走直线」。

6. round-trip 与 golden 对照

往返测试：Log(Exp(x)) 与 Exp(Log(R)) 在随机一千样本（含 π 邻域、极小角）最大误差 $< 1 0^{- 10}$ ，否则别上预积分雅可比简化。库实现小角 Taylor、大角 SVD——两套实现要对 golden 位姿。预积分雅可比简化版与数值微分对照，π 附近不 flip。单元测试应单独覆盖 π 邻域与极小角，随机均匀采样往往漏掉奇异邻域。

7. 案例：预积分简化角函数拖垮闭环

某套自研预积分在小角区间把正弦换成线性近似以省算力，仿真里轨迹正常，长走廊 field 里 yaw 在回环后缓慢偏三度。换成全角正弦并每步归一化四元数后，同一 bag 闭环成功。根因不是回环检测，是李群 helper 在切空间与协方差更新上不一致。教训：任何「近似」必须写进 spec 并用 golden 覆盖 π 与极小角。

8. 验收

插值全程 $∣ q ∣ - 1 ∣< 1 0^{- 10}$ 。
日志打印浮点带足够位数，别截断后误判相等。
golden 位姿与库输出逐元素对比。
预积分与优化器共用同一套 Exp/Log 实现，禁止两套近似并存。

李群 helper 写错一次，优化器「收敛」到错姿态——数值卫生是 field 轨迹能不能闭合的分界线，不是论文里的脚注。