A Lagrange-Newton Approach to Smoothing-and-Mapping

In this report we explore the application of the Lagrange-Newton method to the SAM (smoothing-and-mapping) problem in mobile robotics. In Lagrange-Newton SAM, the angular component of each pose vector is expressed by orientation vectors and treated through Lagrange constraints. This is different from the typical Gauss-Newton approach where variations need to be mapped back and forth between Euclidean space and a manifold suitable for rotational components. We derive equations for five different types of measurements between robot poses: translation, distance, and rotation from odometry in the plane, as well as home-vector angle and compass angle from visual homing. We demonstrate the feasibility of the Lagrange-Newton approach for a simple example related to a cleaning robot scenario.