Incremental SQP method for constrained optimization formulation in SLAM

The simultaneous localization and mapping (SLAM) problem has been a research focus for many years and have reached a mature state. However, more robust solutions to the SLAM problem are still required, especially in large noise level scenarios. Because of the strong non-linearity of the SLAM problem, it is vital to start from a good initial value to avoid being trapped in local minima. In this paper, we propose a new SLAM formulation transforming the unconstrained Least Squares formulation into a constrained optimization problem. Algorithms based on this new formulation can naturally start from good initial value. Different from other constrained optimization problem, this new formulation can be efficiently solved with Sequential Quadratic Programming (SQP) methods. Based on SQP, we propose an incremental SQP algorithm to solve SLAM, which shows great advantage over Gauss Newton (g2o implementation) when working in large noise level scenarios. Experimental results show the validity of the proposed approach.