IEEE Transactions on Robotics

On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics

作者：Connor Holmes, Frederike Dümbgen, Timothy Barfoot · 机构：Robotics Institute, University of Toronto, Toronto, ON, Canada; Inria Paris, École Normale Supérieure, PSL University, Paris, France

会议/期刊：IEEE Transactions on Robotics · 发布日期：2024 · 页码：4805-4824

优化定位建图状态估计

精读笔记

Problem Setting

这篇论文真正解决的是 certifiable state estimation 在真实噪声模型下的脆弱性问题。过去很多 robotics SDP relaxation 的成功案例默认 isotropic/scalar weights，这让 cost 和 constraints 具有很好的代数性质：二次形式、旋转不变性、变量可消元、低秩 SDP 容易 tight。但真实传感器，尤其 stereo camera，把 pixel/disparity 转成 3D Euclidean pseudo-measurement 后，协方差天然沿深度方向拉长，权重应是矩阵而不是标量。

真正困难点在于：matrix weights 不是一个无害的建模改进。它会破坏标量情形下很多“隐式成立”的结构。对 localization/Wahba，问题仍可写成 QCQP，但 tightness 对噪声和观测几何变得敏感；对 landmark-based SLAM，矩阵权重使得 landmark residual 中的 C_i m_k 无法通过旋转不变性线性化，原 scalar-weighted SLAM relaxation 不能直接使用。关键矛盾是：更真实的噪声模型提高了统计建模正确性，却削弱了全局优化可认证性的结构基础。

Motivation

已有路线不够的地方很明确：certifiable perception 社区经常把 isotropic noise 当作工程简化，但在 stereo、视觉 landmark、某些几何 primitive registration 中，这个假设不仅不真实，而且会改变最大似然问题本身。若继续用 scalar weights，优化问题更容易 certifiable，但估计器不是正确的 ML estimator；若使用 matrix weights，估计模型正确了，却可能失去 SDP tightness 和低秩求解效率。

作者的核心观察是：tightness 不是只由 noise magnitude 决定，还由 noise covariance geometry 决定。各向异性本身未必致命，致命的是多个测量的不确定方向对齐，从而在 posterior 中形成某个方向上的高不确定性。这个观察把“SDP relaxation 为什么不 tight”从抽象代数问题拉回到 state estimation 中熟悉的 FIM degeneracy / posterior uncertainty 语言，这是论文最有价值的动机来源。

Core Idea

论文的核心思想是：不要把 matrix-weighted relaxation 的失败看成偶然数值现象，而要把它解释为测量几何导致的 posterior uncertainty 在 lifted SDP 空间中的反映。作者把 matrix-weighted localization 和 SLAM 重新写成标准 QCQP，再研究其 SDP relaxation 的 primal rank 和 dual certificate matrix。关键变化是，从“标量权重下的已知 tight formulation”转向“真实 anisotropic measurement 下 tightness 需要额外约束维持”的建模范式。

本质区别在于，prior work 多数是在 favorable algebraic structure 下发展快速 certifiable solver；这篇论文则指出这种 favorable structure 的边界在哪里，并给出一条补救路线：添加在原问题中冗余、但在 SDP lifted space 中非冗余的约束。这个 inductive bias 很具体：强制 lifted solution 遵守更多 SO(3) 和 landmark-pose 几何一致性，从而压缩 SDP relaxation 中不对应真实 feasible solution 的自由度。

Method

1. Matrix-weighted localization/Wahba formulation：它解决的是各向异性 landmark measurements 下单姿态或多姿态 localization 的正确 ML 建模问题。由于 landmark 已知，residual 对 C_i 和 t_i 仍是线性的，因此 cost 仍可写成 QCQP。核心变化是 scalar closed-form Wahba 不再适用，需要 SDP 或局部迭代；而 SDP 是否 tight 取决于 noise geometry。

2. Matrix-weighted SLAM formulation：它解决的是 landmarks 未知时 matrix weights 破坏 scalar SLAM relaxation 的问题。标量权重下可以利用旋转保持范数，把 residual 变成线性形式；矩阵权重下 C_i^T W C_i 不等于 W，这个 trick 失效。作者引入每个观测对应的 substitution variable m_i^k，并用约束 m_i^k = C_i m_0^k - t_i 维持几何一致性。核心变化是把 quartic cost 转回 QCQP，但代价是变量和约束显著增加。

3. Redundant constraints：它解决的是基础 SDP relaxation 不 tight 的问题。这些约束包括额外 SO(3) 约束、行列范数关系、landmark 间距离/相对位移在不同坐标系中的一致性等。它们在原非凸问题中不会改变 feasible set，但在 SDP relaxation 中排除了大量 spurious lifted solutions。核心变化是用更多几何一致性换 tightness。

4. FIM-certificate connection：它解决的是 tightness 退化缺少可解释机制的问题。作者证明在局部参数化与 QCQP 表达等价时，FIM 可写成 L^T H L，其中 H 是 SDP dual certificate matrix，L 是从局部参数到 lifted variable 的 Jacobian。这把 posterior uncertainty 和 certificate spectrum 联系起来，使得 anisotropic uncertainty 对 tightness 的影响有了理论解释。

Key Insight / Why It Works

最重要的 insight 是：SDP tightness 的退化与估计 posterior 的退化不是两个独立现象。FIM 的小特征值意味着在某些状态方向上信息弱；通过 L^T H L 的关系，这种弱信息会反映到 certificate matrix 的谱结构中。当 H 的低特征值变得过小，SDP relaxation 更容易出现高 rank 解或 dual certificate 失效。换句话说，anisotropic noise 不是因为“权重矩阵更复杂”而破坏 tightness，而是因为它把信息集中/缺失组织成了更尖锐的几何方向。

这也解释了实验现象：如果所有 uncertainty ellipsoids 的长轴对齐，posterior 在某个方向上高度不确定，tightness 很快丢失；如果 ellipsoids 方向分散，弱方向互补，tightness 可以维持到更高噪声。增加 landmark 数量有效，不是简单 scaling，而是增加几何覆盖、提升 FIM 的最小信息方向。这里的核心不是 data volume，而是 measurement geometry diversity。

Redundant constraints 有效的原因也很清楚：它们不是改善统计信息，而是改善 relaxation 的几何表达能力。原问题中的 SO(3)、坐标变换、landmark 相对距离等关系在 rank-one feasible point 上自动成立，但 SDP lift 后 rank-one 结构被放松，这些关系不再自动成立。加入 redundant constraints 等于把部分 rank-one 隐含结构显式写回 relaxation。它们提升的是 relaxation fidelity，而不是 estimator 本身的信息量。

我认为论文中最核心的贡献是 FIM-certificate 连接与 matrix-weighted SLAM formulation failure 的指出。redundant constraints 的具体集合很有用，但更像必要的 engineering/theory bridge：它证明问题可补救，却没有从根本上解决 scalable certification。真实系统实验中 matrix-weighted global solver 避免局部极小很有说服力，但精度收益并不强，且可能被前端、训练数据、ground truth 误差掩盖；这部分不能过度解读为 matrix weighting 在 pipeline 级别显著优于 scalar baseline。

Relation To Prior Work

这篇论文处在 SE-Sync / Cartan-Sync / certifiable SLAM / rotation synchronization / SDP relaxation 这条技术谱系中，但它不是继续追求更快 solver，而是在拆解这些 solver 成立的建模前提。和 SE-Sync 类工作相比，差异不在于 SDP 形式本身，而在于它研究了当 isotropic assumption 被移除后，tightness 和低秩结构如何崩坏。

和已有 redundant constraint 工作相比，比如 relative pose、essential matrix、3D registration、hand-eye calibration 等，本文的新意不是“发现 redundant constraints 能 tighten SDP”这个事实；这个思想已有。实质新增在于：把 redundant constraints 放到 matrix-weighted robotics state estimation 的语境里，系统展示它们何时成为必要条件，并用 posterior uncertainty/certificate matrix 的关系解释 tightness 退化。

和 scalar-weighted landmark-SLAM relaxation 的关系尤其关键。过去 scalar SLAM 的 tightness 经验很好，但本文指出那不是 SLAM 本身天然容易，而是 scalar weights 保留了旋转不变性和二次结构。matrix weights 一进入，原 formulation 甚至不可用。这是一个比较强的负结果，对后续可认证 SLAM 的建模假设有实际警示意义。

Dataset / Evaluation

实验设计主要服务于机制验证，而不是展示一个大规模系统。模拟实验覆盖了 aligned/misaligned anisotropic ellipsoids、stereo camera covariance、不同 landmark 数量/距离/分布，对核心 claim 的支撑较充分：tightness 依赖噪声几何，redundant constraints 能显著恢复 tightness，stereo 场景下不加 redundant constraints 通常不可靠。

真实实验分两类。Outdoor stereo localization 用 In-the-Dark 数据集验证 matrix-weighted Wahba SDP 可以嵌入实际 pipeline，并在局部优化失败时给出全局解；这是对 localization 子问题的现实可用性证明。Starry Night stereo SLAM 则只在小规模 pose/landmark 子集上验证 redundant constraints 的必要性和有效性，更像 controlled demonstration。

评价的主要 limitation 是没有证明 matrix-weighted SLAM 可扩展到真实大规模在线 SLAM。论文自己也承认 SDP dimension 和 constraints 数量很快不可承受。此外，localization 中 scalar SVD baseline 在表格上并不差，甚至在某些 RMSE 上更好；因此实验更支持“matrix-weighted SDP 能认证并避免坏局部极小”，而不是强支持“matrix-weighted 在真实 pipeline 中显著提升精度”。

Limitation

第一，scalability 是硬上限。matrix-weighted Wahba 因为单姿态问题维度小，可以直接用 interior-point SDP；但 matrix-weighted SLAM 引入 substitution variables 和 redundant constraints 后，约束数量爆炸。这里不是实现没优化的问题，而是 formulation 本身把问题复杂度转移到了 lifted SDP 空间。

第二，redundant constraints 的选择仍有经验色彩。作者用 concurrent work 的 constraint-learning 方法在小实例上发现数值约束，再人工解释成代数约束并迁移到一般实例。这很实用，但文中未充分说明该 constraint set 对任意图结构、任意观测几何、任意噪声水平的充分性边界。它更像“empirically sufficient constraint family”，不是完整理论刻画。

第三，FIM-certificate connection 给出了漂亮解释，但对 tightness threshold 的预测仍不完整。它能解释 degeneracy 和 tightness loss 的相关性，但从 FIM spectrum 定量推出 SDP rank behavior 仍未解决。文中关于近退化情形的机制仍主要是经验观察。

第四，真实系统增益归因不完全清楚。local solver 的坏局部极小说明 global optimization 有价值，但 scalar baseline 表现强，matrix weighting 的精度收益并不显著。增益到底来自正确噪声建模、全局求解、还是避免少数局部失败，文中没有完全拆开。对 deployment 来说，这会影响是否值得付出 SDP 成本。

Takeaway

  • 1. Certifiable perception 的 tightness 不能脱离噪声模型讨论；isotropic noise 不是小假设，而是很多 relaxation tightness 的结构性来源。
  • 2. Matrix weighting 的主要风险不是数值条件变差，而是破坏旋转不变性和低秩 SDP 的 favorable geometry；尤其在 SLAM 中会迫使 formulation 改写。
  • 3. Redundant constraints 是恢复 tightness 的有效工具，但当前更像用计算成本换建模真实性。
  • 未来真正关键的是让 redundant-constraint SDP 也能低秩、稀疏、可在线认证。

一句话总结

这篇论文是 certifiable robotics 从理想 isotropic 建模走向真实 anisotropic sensing 时的一篇边界论文：它证明 matrix weights 会系统性破坏现有 SDP relaxation 的 tightness，并指出恢复 tightness 需要显式几何冗余约束，但 scalable 解法仍未解决。