A Lagrange-Newton Approach to Smoothing-and-Mapping

Smoothing-and-mapping (SAM) is a fairly modern approach to building graph-based maps of the environment; a tutorial is given by Grisetti et al. (2010) (see also the tutorial on Newton-type methods with applications in graph-based SLAM by Toussaint (2017)). Following the tutorial by Grisetti et al. (2010), the SAM problem (1) is solved in a Gauss-Newton framework and (2) involves transformations between manifolds for the rotational components. In the Gauss-Newton framework, error vectors (between true and expected measurements) are approximated to first order by a Taylor expansion, thus the resulting scalar error terms are second-order equations. A Newton descent is then performed based on the approximated Hessian (computed from the Jacobian) in these equations. However, while the Gauss-Newton framework allows for straightforward derivations, it suffers from a drawback with respect to rotational cost functions: In the quadratic approximation of the scalar error functions, the cyclic character of angular terms is lost. Therefore, to handle rotational components (e.g.