FSM: Correspondenceless scan-matching of panoramic 2D range scans

I/O · install · run

fsm_localisation is a ROS 2 package written in C++ that matches 2D lidar scans. One node, two modes, one matcher underneath both. Odometry mode expects a sensor whose field of view is 360 degrees. Amelioration mode also reads a sensor that covers less.

Odometry mode matches each scan against the one before it and reports how far the sensor moved. No map and no pose estimate are needed.

Amelioration mode matches a scan against an occupancy grid and reports where the sensor actually was, correcting a pose estimate somebody else produced.

A sensor that does not see all the way round is read in amelioration mode. The map is read over a whole turn at the sensor’s own angular spacing and only the part of it the sensor could have seen is compared. The scan has to say what it covers, through angle_increment and a ray count, and has to be centred on the sensor’s heading; a scan that spans more than a turn, or that begins somewhere else, is refused at the point of use rather than answered wrongly. Odometry mode does not read what a scan covers. It lays every scan out over a whole turn, so it expects a sensor that sees all the way round. It does not refuse a narrower one.

Both are achieved by scan-matching without establishing correspondences between elements of the input scans or their properties, but by leveraging the range signal’s periodicity. Hence FSM may exploit properties of the Discrete Fourier Transform. These two pillars support the robustness of FSM’s pose error against (a) sensor noise and (b) distance between consecutive poses, as exhibited in the figure which summarises key experiments below.

Why use FSM

Table of Contents

• Requirements
• Installation
  • With Docker
  • From source
• Run
  • Launch
  • Both modes at once
  • Call
• The two modes
  • Odometry mode
  • Amelioration mode
• Parameters
  • Shared by both modes
  • Odometry mode only
  • Amelioration mode only
• What the map costs and what it bounds
• The borrowed ray casting
• Diagnostics
• Coming from fsm_lidar_odometry
• Upgrading from the ROS 1 version
• Motivation and Under the hood

Requirements

ROS 2 Lyrical and a compiler with C++23 support. Beyond ROS the package needs FFTW3, CGAL and Eigen3. All three are declared in the package manifest, so rosdep pulls them in without being asked.

This package is MIT, but two libraries it links are not. The CGAL components it uses are GPL v3 and FFTW is GPL v2 or later. A binary built from these sources therefore carries GPL v3 terms if you redistribute it.

The vendored ray casting under include/fsm_localisation/range_libc/ is Apache 2.0 and is governed by the licence file beside it rather than by this package’s MIT terms. See The borrowed ray casting.

Installation

Two ways. There is no apt package and no published image: this package is built from these sources.

With Docker

The image carries ROS 2 Lyrical, every dependency and a built workspace, so the host needs nothing but Docker. It is a runtime image: it holds what the node needs to run and no visualisation or simulation tools, which keeps it a little over two gigabytes.

docker compose -f docker/docker-compose.yml build

Two build arguments change the arithmetic, and both default to off, which is what this package ships as:

docker compose -f docker/docker-compose.yml build \
  --build-arg EXACT_FP=ON --build-arg DETERMINISTIC_FFT=ON

EXACT_FP=ON forbids the compiler from reassociating floating point arithmetic, so two compilers give comparable numbers. DETERMINISTIC_FFT=ON makes FFTW pick its plan by heuristic rather than by timing itself, so two runs of one binary give the same numbers. Both cost speed. Turn them on to compare builds, not to run a robot.

Whoever builds this package settles the arithmetic for everything above it. The choice is recorded where a dependent package reads it, so a package built against this one follows it rather than making its own. See As a dependency of your own package.

docker/shared-input and docker/shared-output on the host are mounted into the container, for feeding it a recording and taking one back.

From source

```bash mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src git clone

File truncated at 100 lines see the full file