How-to Augmented Lagrangian on Factor Graphs

Robotics is progressively pervading our daily-lives. Robots can be divided in two large categories: manipulators and mobile robotics. In this thesis, we focus on the latter. An autonomous robotics navigation system needs to address many tasks. Simultaneous Localization and Mapping (SLAM) is the task of simultaneously build a map of the environment and localizing in it, control is the task of computing the platform inputs, which can be acceleration, velocity, jerk, that allow the robot to reach a given goal, high-level planning is the more abstract task of selecting a sequence of goals that allow the robot to perform the high-level task, e.g. mapping an area, driving to a final position, rescue survivors to an accident, and many others. In a divide-and-conquer approach, each task is delegated to a sub-module. These components have been investigated separately by different research communities, that has developed sophisticated tools to address the sub-problems of interest. In particular, the SLAM community consolidated the Maximum-A-Posteriori approach, and the usage of factor graphs as an effective tool to model and solve large mapping problems. Whatever the problem is, real-world, long-term and broad diffusion of robotic systems require the tools to be efficient, robust and resource-aware. Factor graphs are graphical models used to represent unconstrained optimization problems that combine many objectives, each of which only depends on a subset of variables. Extending factor graphs to constrained optimization allows to enlarge their application domain. Under the factor graph formulation, control systems can benefit from the efficiency of existing factor graph-solvers, the ease of modeling and of combining objectives. Moreover, some estimation problems inherently involve constraints in their formulation, that can be straightforwardly embedded in the graph. Potentially, mapping and control sub-modules can be unified under one common language, which implies that they can share consistently information about the environment. In this thesis, we investigate how to embed constraints in the factor graph-formalism. We introduce a generalized version based on the Augmented Lagrangian (AL) method, where edges can be objective terms or constraints. We support our methodology by presenting many applications, ranging from group synchronization, which arises for example as sub-problem of Structure from Motion (SfM), to optimal control. The contributions presented in this thesis are available as open-source software packages to foster the development of this research area.