Factor graphs are graphical models used to represent a wide variety of problems across robotics, such as Structure from Motion (SfM), Simultaneous Localization and Mapping (SLAM) and calibration. Typically, at their core, they have an optimization problem whose terms only depend on a small subset of variables. Factor graph solvers exploit the locality of problems to drastically reduce the computational time of the Iterative Least-Squares (ILS) methodology. Although extremely powerful, their application is usually limited to unconstrained problems. In this paper, we model constraints over variables within factor graphs by introducing a factor graph version of the method of Lagrange Multipliers. We show the potential of our method by presenting a full navigation stack based on factor graphs. Differently from standard navigation stacks, we can model both optimal control for local planning and localization with factor graphs, and solve the two problems using the standard ILS methodology. We validate our approach in real-world autonomous navigation scenarios, comparing it with the de facto standard navigation stack implemented in ROS. Comparative experiments show that for the application at hand our system outperforms the standard nonlinear programming solver Interior-Point Optimizer (IPOPT) in runtime, while achieving similar solutions.
翻译:系数图形是用来代表各种机器人之间问题的广泛不同的图形模型,例如来自运动的结构(SfM)、同声本地化和绘图(SLAM)和校准。通常,它们的核心有一个优化问题,其条件仅取决于一小部分变量。系数图形解析器利用问题地点来大幅度缩短Liative-Squares(ILS)方法的计算时间。虽然其应用能力非常强大,但通常仅限于不受约束的问题。在本文中,我们通过引入一个要素图形版本的Lagrange多动器方法(SLAM)和校准校准等,对要素图形中的变量进行模型限制。我们通过在要素图形上展示一个完整的导航堆来展示我们的方法的潜力。不同于标准的导航堆,我们可以用要素图形模拟当地规划和本地化的最佳控制,用标准 ILS 方法解决两个问题。我们验证了在现实世界自主导航情景中的方法,将其与在 ROS 中执行的事实上标准导航堆进行对比。比较实验显示我们的方法的潜力,我们的方法是用一个基于要素图形图的系统,在运行式的系统上,在运行式的OPDPD(Opresmol),同时实现标准的不动式解的不动式解。