Factor graphs are a very powerful graphical representation, used to model many problems in robotics. They are widely spread in the areas of Simultaneous Localization and Mapping (SLAM), computer vision, and localization. In this paper we describe an approach to fill the gap with other areas, such as optimal control, by presenting an extension of Factor Graph Solvers to constrained optimization. The core idea of our method is to encapsulate the Augmented Lagrangian (AL) method in factors of the graph that can be integrated straightforwardly in existing factor graph solvers. We show the generality of our approach by addressing three applications, arising from different areas: pose estimation, rotation synchronization and Model Predictive Control (MPC) of a pseudo-omnidirectional platform. We implemented our approach using C++ and ROS. Besides the generality of the approach, application results show that we can favorably compare against domain specific approaches.

翻译：因子图是一种强大的图形表示方式，常用于机器人领域的诸多问题建模，广泛应用于同步定位与地图构建（SLAM）、计算机视觉及定位等领域。本文提出一种方法，通过将因子图求解器扩展至约束优化，填补其与最优控制等其他领域之间的空白。该方法的核心思想是将增广拉格朗日方法封装在因子图的因子节点中，从而可无缝集成至现有因子图求解器。我们通过处理三个不同领域的应用场景——位姿估计、旋转同步及伪全向平台模型预测控制——证明了方法的通用性。基于C++与ROS实现后，应用结果不仅展示了方法的普适性，还表明其性能可与领域专用方法相媲美。