让因子图优化支持等式约束，提升自动驾驶轨迹跟踪精度。
ecg2o: A Seamless Extension of g2o for Equality-Constrained Factor Graph Optimization
- 在g2o基础上无缝加入等式约束，无需改用新算法。
- 在车辆速度追踪任务中优于现有约束处理方法。
- 开源库ecg2o即插即用，适合机器人与控制研究者使用。
因子图优化是机器人感知的核心框架，广泛应用于位姿估计、同步定位与建图（SLAM）、运动恢复结构（SfM）和态势感知。传统方法通过高斯-牛顿或列文伯格-马夸尔特等算法求解无约束最小二乘问题。然而，为因子图原生引入等式约束可提升解的精度并拓展其应用范围，尤其在最优控制领域。本文提出一种新的因子图扩展方法，无需额外优化算法即可无缝集成等式约束，同时保持二阶优化技术的高效性与灵活性，并确保约束可行性。我们将其应用于自主车辆速度追踪的最优控制问题，并与当前先进约束处理技术进行对比验证。此外，我们发布了ecg2o——一个基于g2o的纯头文件C++库，完整支持等式约束优化。该库及示例代码已开源，地址为https://github.com/snt-arg/ecg2o。
原文摘要 · Abstract (English)
Factor graph optimization serves as a fundamental framework for robotic perception, enabling applications such as pose estimation, simultaneous localization and mapping (SLAM), structure-from-motion (SfM), and situational awareness. Traditionally, these methods solve unconstrained least squares problems using algorithms such as Gauss-Newton and Levenberg-Marquardt. However, extending factor graphs with native support for equality constraints can improve solution accuracy and broaden their applicability, particularly in optimal control. In this paper, we propose a novel extension of factor graphs that seamlessly incorporates equality constraints without requiring additional optimization algorithms. Our approach maintains the efficiency and flexibility of existing second-order optimization techniques while ensuring constraint feasibility. To validate our method, we apply it to an optimal control problem for velocity tracking in autonomous vehicles and benchmark our results against state-of-the-art constraint handling techniques. Additionally, we introduce ecg2o, a header-only C++ library that extends the widely used g2o factor graph library by adding full support for equality-constrained optimization. This library, along with demonstrative examples and the optimal control problem, is available as open source at https://github.com/snt-arg/ecg2o
Thank you to arXiv for use of its open access interoperability. PaperDance 不是 arXiv 官方产品；中文卡片由大模型生成，请以原文为准。