A back-end L<inf>1</inf> norm based solution for factor graph SLAM

摘要

Graphical models jointly with non linear optimization have become the most popular approaches for solving SLAM and Bundle Adjustment problems: using a non linear least squares (NLSQs) description of the problem, these math tools serve to formalize the minimization of an error cost function that relates state variables through relative sensor observations. The simplest case just considers as state variables the locations of the sensor/robot in the environment deriving in a pose graph subproblem. In general, the cost function is based on the L <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2</sub> norm whose principal iterative solutions exploit the sparse connectivity of the corresponding Gaussian Markov Field (GMRF) or the Factor Graph, whose adjacency matrices are given by the fill-in of the Hessian and Jacobian of the cost function respectively. In this paper we propose a novel solution based on the L <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">1</sub> norm as a back-end to the pose graph subproblem. In contrast to other NLSQs approaches, we formulate an iterative algorithm inspired directly on the Factor Graph structure to solve for the linearized residual ∥Ax - b∥ <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">1</sub> . Under the presence of spurious measurements the L <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">1</sub> based solution can achieve similar results to the robust Huber norm. Indeed, our main interest in L <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">1</sub> optimization is that it opens the door to the set of more robust non-convex L <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">p</sub> norms where p ≤ 1. Since our approach depends on the minimization of a non differentiable function, we provide the theoretical insights to solve for the L <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">1</sub> norm. Our optimization is based on a primal-dual formulation successfully applied for solving variational convex problems in computer vision. We show the effectiveness of the L <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">1</sub> norm to produce both a robust initial seed and a final optimized solution on challenging and well known datasets widely used in other state of the art works.