Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

A Lagrange-Newton Approach to Smoothing-and-Mapping

Ralf Möller

TL;DR

This work tackles planar smoothing-and-mapping (SAM) by adopting a full Newton descent, expressing rotations through unit orientation vectors and enforcing $\|\mathbf{u}_i\|=1$ with Lagrange multipliers. By deriving exact first- and second-order derivatives for five measurement costs (translation, distance, odometry rotation, home-vector, compass) and embedding them in a Lagrange-Newton framework, the method avoids the usual manifold-transformations required by Gauss-Newton on rotations. The approach is demonstrated on a simple cleaning-robot scenario, showing feasibility and robustness to small constraint violations, while highlighting sensitivity to initial Lagrange multipliers and occasional numerical instabilities. Overall, the proposed orientation-vector formulation with an augmented-Lagrangian Newton solver provides a principled alternative to manifold-based rotation handling in planar SAM, with potential extension to 3D and more complex environments.

Abstract

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.

A Lagrange-Newton Approach to Smoothing-and-Mapping

TL;DR

This work tackles planar smoothing-and-mapping (SAM) by adopting a full Newton descent, expressing rotations through unit orientation vectors and enforcing with Lagrange multipliers. By deriving exact first- and second-order derivatives for five measurement costs (translation, distance, odometry rotation, home-vector, compass) and embedding them in a Lagrange-Newton framework, the method avoids the usual manifold-transformations required by Gauss-Newton on rotations. The approach is demonstrated on a simple cleaning-robot scenario, showing feasibility and robustness to small constraint violations, while highlighting sensitivity to initial Lagrange multipliers and occasional numerical instabilities. Overall, the proposed orientation-vector formulation with an augmented-Lagrangian Newton solver provides a principled alternative to manifold-based rotation handling in planar SAM, with potential extension to 3D and more complex environments.

Abstract

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.
Paper Structure (42 sections, 125 equations, 2 figures)

This paper contains 42 sections, 125 equations, 2 figures.

Table of Contents

  1. Introduction
  2. Notation
  3. Orientation Vectors
  4. Cost Functions
  5. Pose Vectors
  6. Cost Functions and Constraint
  7. Generic Rotational Cost Function
  8. Odometry Measurement
  9. Translation Error
  10. Distance Error
  11. Rotation Error
  12. Homing Measurement
  13. Home-Vector Error
  14. Compass Error
  15. Derivatives
  16. ...and 27 more sections

Figures (2)

  • Figure 1: Cost functions. Each diagram shows the computation of the cost term for a pair of poses in the plane (red and blue dots and orientation vectors). Green distances and green angles visualize errors. The uncertainty is only visualized for the translation error (error ellipse); for the other cost functions, the one-dimensional translatory and angular uncertainties are not shown.
  • Figure 2: Simulation experiment. Robot poses are shown by circles with a bar indicating the forward direction. Top: Estimated poses (red), true poses (black), and pairs of poses used in homing measurements (dotted green lines). The first lane starts at the origin, subsequent lanes are shifted upwards. Bottom: Result of the Lagrange-Newton descent overlaid on the top figure. Initial state (cyan), final state (blue), pairs of poses used in homing measurements (dotted green lines). (Inter-lane connections in the initial and final states should be ignored.)