Robust pose graph optimization with loop closure outliers

Abstract

With the increasing need of autonomous robots for complicated environment situation such as underwater application, more robust algorithm is needed. In simultaneous localization and mapping algorithm, one of the core parts is the back end. The noisy measurement and robot trajectory are process to correct the drifting error using loop closure measurement (recognition of previously visited place). The process of optimizing the robot poses with respect to the sensor measurements is called pose graph optimization (PGO). Solving a PGO problem is equivalent to solve a maximum likelihood estimation problem where the objective function is the error between the measurement and the poses. The classical framework is to use a least-square formulation. However, this formulation has several drawbacks: The sensitivity to poses initialization first can lead to a local minima solution as it is a nonconvex problem. Then the presence of wrong measurement with large error, also called outliers, can lead to arbitrary wrong solution. In this research, we aim at studying a method for PGO which leverages the problem of initialization and is robust to outliers presence. The initialization sensitivity problem comes from the nonconvexity of the minimization problem as it introduces multiple local minima. The proposed solution is to relax the problem into a convex one with a single global minimum. The solution of the relaxed problem can be reprojected on the initial nonconvex problem feasible set. Additionally, using this method we have a contract on the certifiability of our solution, i.e we can ensure that the solution is the global minima, or we detect the failure. For the sensitivity to outliers, it mainly comes from the fact that the formulation is quadratic in the error terms so if one measurement contains a wrong large error it will dominate the objective function. The proposed approach here is the use of M-estimator. A M-estimator is adding a loss function around the error term to mitigate its impact if it is too large. This thesis aims at comparing different loss function that can be used on the chosen convex relaxation approach. Additionally, we suppose that only edge which are loop closure can be outliers. After deriving the formulation corresponding to our choice, we test on 3 synthetic datasets the different loss function and compare them. Our results show that the convex loss function, i.e L1, L2, identity tested here do well for highly connected pose graph but failed to stay robust for low connectivity pose graph.