ROS 2 LOAM Scan Matching Node

Overview

This script implements a ROS 2 node, loam_scan_matcher, that subscribes to edge and planar feature point clouds and performs incremental pose estimation using point-to-plane Iterative Closest Point (ICP) registration. The node integrates relative transformations into a global pose, continuously publishing odometry, trajectory, and frame transforms.

The algorithm follows the LOAM (LiDAR Odometry and Mapping) framework by Zhang & Singh (2014) and is designed for real-time incremental LiDAR odometry. It has been adapted from ENPM818Z L2C lecture content and is suitable for research and teaching applications within the NIST SLAM Front-End testbed.

Core Functionality

The node performs the following sequence of operations for each incoming frame:

  1. Feature Fusion Combines edge features (high curvature) and planar features (low curvature) from the current scan into a single point cloud for registration.

  2. Voxel Downsampling Reduces point density using voxel-grid hashing to improve ICP convergence speed and robustness.

  3. Normal Estimation Computes per-point surface normals on the reference frame using k-NN PCA. Normals are oriented consistently (toward +Z by heuristic) for reliable point-to-plane residuals.

  4. Point-to-Plane ICP Registration Performs iterative closest point matching against the previous frame using point-to-plane constraints:

    \[\mathbf{n}_i^T \left( R\mathbf{q}_i + \mathbf{t} - \mathbf{p}_i \right) \approx 0\]

    where \(\mathbf{p}_i\) are reference points, \(\mathbf{q}_i\) are source points, \(\mathbf{n}_i\) are surface normals, and the goal is to estimate \(R\) and \(\mathbf{t}\).

  5. SE(3) Optimization Solves the registration problem directly in Lie algebra (se(3)) using the exponential map:

    \[\begin{split}\exp(\boldsymbol{\xi}) = \begin{pmatrix} R & V\mathbf{t} \\ 0 & 1 \end{pmatrix}\end{split}\]

    where \(\boldsymbol{\xi} = [\boldsymbol{\omega}, \mathbf{t}]^T\) is a 6D twist vector. This allows direct least-squares optimization without singularities.

  6. Pose Integration Integrates the estimated relative transformation into the global world-to-base transformation:

    \[T_{wb} \gets T_{wb} \cdot T_{curr}\]

  7. Publishing The node publishes:

    • Odometry (scan_match/odometry): Current pose in the map frame as a nav_msgs/msg/Odometry message.

    • Trajectory (scan_match/trajectory): Accumulated path as a nav_msgs/msg/Path message.

    • TF Transform (map → base_link): Broadcast pose as a geometry frame transform.

Coordinate Frame Hierarchy

The scan matcher uses a single global frame hierarchy rooted in the map frame:

map (global reference)
  └── base_link (robot body frame)
        └── velodyne (LiDAR sensor frame)

Frame Semantics:

  • map: Global reference frame provided by the KITTI ground truth publisher. All poses and point clouds are ultimately expressed in this frame.

  • base_link: Robot body frame. Updated by the scan matcher with the estimated pose T_wb (world-to-body transformation).

  • velodyne: LiDAR sensor frame. Point clouds are published in this frame by the KITTI publisher and feature extractor.

Why Clouds Appear in Both Frames (Foxglove Display):

When visualizing point clouds in Foxglove Studio, the same cloud data can be displayed in multiple frames because:

  1. Point clouds carry a frame_id (e.g., velodyne) indicating their native frame.

  2. Foxglove uses the TF tree to transform clouds into any requested frame.

  3. Selecting different panels with different frameId settings will show the same cloud data transformed into those frames.

For example: - A cloud native to velodyne displayed in the map frame is transformed as: velodyne → base_link → map - The same cloud displayed in the base_link frame is transformed as: velodyne → base_link

If the map → base_link transform is static (pose not changing), the clouds will appear identical in both frames. If the transform updates with new pose estimates, the clouds will appear to move relative to the map frame grid.

Note: To verify that pose estimation is working, check:

ros2 topic echo /scan_match/odometry --field pose.pose.position

Position (x, y, z) should change with each new scan if the sensor is moving.

ROS 2 Interface

Parameters:

LOAM Scan Matcher Parameters

Parameter

Value (from params.yaml)

Meaning

edge_topic

"features/edge_cloud"

Input topic for edge feature clouds from feature extractor.

planar_topic

"features/planar_cloud"

Input topic for planar feature clouds from feature extractor.

voxel_size

0.2

Voxel grid downsampling cell size (meters). Smaller values preserve detail; larger values speed up ICP.

max_icp_iter

20

Maximum number of ICP iterations per frame. Early stopping occurs if update norm < 1e-6.

icp_dist_thresh

1.0

Distance threshold (meters) for considering point-to-point correspondences in ICP. Correspondences beyond this distance are rejected.

knn_normals

20

Number of neighbors used for k-NN surface normal estimation via PCA.

frame_map

"map"

Global reference frame ID (from KITTI ground truth).

frame_base

"base_link"

Robot body frame ID. Updated by scan matcher pose estimates.

odom_topic

"scan_match/odometry"

Output topic for publishing odometry messages.

trajectory_topic

"scan_match/trajectory"

Output topic for publishing trajectory paths.

Subscriptions:

  • <edge_topic> (sensor_msgs/msg/PointCloud2): Edge feature clouds (high curvature points) from the feature extractor.

  • <planar_topic> (sensor_msgs/msg/PointCloud2): Planar feature clouds (low curvature points) from the feature extractor.

Publications:

  • <odom_topic> (nav_msgs/msg/Odometry): Odometry message with pose and frame information in the map frame.

  • <trajectory_topic> (nav_msgs/msg/Path): Accumulated trajectory of all poses since launch.

  • TF Transform (geometry_msgs/msg/TransformStamped): Broadcast transform from <frame_map> to <frame_base>.

Launch Files:

File: src/loam_scan_matcher/launch/scan_matching.launch.py

This launch file starts the complete LOAM scan matching pipeline, including all dependent stages required for full functionality.

It launches the following sequence:

  1. Feature Extraction Pipeline (included) - KITTI data loader - Voxel downsampler - LOAM feature extractor

  2. LOAM Scan Matcher (this file) - Subscribes to edge and planar features - Publishes odometry, trajectory, and TF transforms

loam_scan_matcher/launch/scan_matching.launch.py
  1#!/usr/bin/env python3
  2# -*- coding: utf-8 -*-
  3"""
  4Launch File: scan_matching.launch.py
  5====================================
  6
  7Description
  8-----------
  9This launch file starts the **LOAM scan matching** stage and, by inclusion,
 10its upstream dependencies. It first launches the **feature extraction pipeline**
 11(which itself brings up KITTI playback and LiDAR preprocessing), then launches
 12the scan matcher node configured via a package-local YAML file.
 13
 14Pipeline (launch order)
 15-----------------------
 161) **Feature Extraction Pipeline** (included from
 17   ``loam_feature_extraction/launch/feature_extraction.launch.py``)
 18   - KITTI Data Loader (raw LiDAR + ground truth)
 19   - LiDAR Preprocessing (e.g., voxel downsampling)
 20   - LOAM Feature Extraction (publishes edge/planar feature clouds)
 21
 222) **LOAM Scan Matcher** (this file)
 23   - Subscribes to: ``features/edge_cloud`` and ``features/planar_cloud``
 24   - Publishes: odometry (``/odom``), TF (``odom -> base_link``), and path (``/trajectory``)
 25
 26Configuration
 27-------------
 28All scan matcher parameters are loaded from:
 29``loam_scan_matcher/config/params.yaml``
 30
 31**No inline parameter overrides** are provided in this launch file, ensuring that
 32edits to the YAML fully control behavior (topics, frames, ICP settings, etc.).
 33
 34Execution
 35---------
 36Launch the full chain (KITTI → Preprocessing → Features → Scan Matching):
 37
 38.. code-block:: bash
 39
 40   ros2 launch loam_scan_matcher scan_matching.launch.py
 41
 42Returns
 43-------
 44launch.LaunchDescription
 45    A ROS 2 launch description that includes the feature extraction pipeline
 46    and the LOAM scan matcher node, with parameters sourced from YAML only.
 47"""
 48
 49from launch import LaunchDescription
 50from launch.actions import IncludeLaunchDescription
 51from launch.launch_description_sources import PythonLaunchDescriptionSource
 52from launch.substitutions import PathJoinSubstitution
 53from launch_ros.substitutions import FindPackageShare
 54from launch_ros.actions import Node
 55
 56
 57def generate_launch_description() -> LaunchDescription:
 58    """
 59    Compose the launch description for LOAM scan matching.
 60
 61    Steps:
 62      1) Include the **feature extraction** launch file from the
 63         ``loam_feature_extraction`` package so that KITTI playback and LiDAR
 64         preprocessing are available before scan matching starts.
 65      2) Launch the **scan matcher** node from the ``loam_scan_matcher`` package,
 66         loading its configuration solely from ``config/params.yaml``.
 67
 68    Returns:
 69        launch.LaunchDescription: The composed launch description.
 70    """
 71    # ------------------------------------------------------------------
 72    # Include LOAM Feature Extraction Pipeline
 73    # (KITTI loader + preprocessing + feature extractor)
 74    # ------------------------------------------------------------------
 75    feature_extraction_launch = IncludeLaunchDescription(
 76        PythonLaunchDescriptionSource(
 77            PathJoinSubstitution([
 78                FindPackageShare("loam_feature_extraction"),
 79                "launch",
 80                "feature_extraction.launch.py",
 81            ])
 82        )
 83    )
 84
 85    # ------------------------------------------------------------------
 86    # Scan Matcher parameters from YAML (no inline overrides)
 87    # ------------------------------------------------------------------
 88    scan_matcher_params = PathJoinSubstitution([
 89        FindPackageShare("loam_scan_matcher"),
 90        "config",
 91        "params.yaml",
 92    ])
 93
 94    # ------------------------------------------------------------------
 95    # LOAM Scan Matcher Node
 96    # ------------------------------------------------------------------
 97    scan_matcher_node = Node(
 98        package="loam_scan_matcher",
 99        executable="loam_scan_matcher",
100        name="loam_scan_matcher",
101        output="screen",
102        parameters=[scan_matcher_params],  # <-- YAML-driven configuration only
103    )
104
105    # ------------------------------------------------------------------
106    # Launch: feature extraction first, then scan matcher
107    # ------------------------------------------------------------------
108    return LaunchDescription([
109        feature_extraction_launch,
110        scan_matcher_node,
111    ])

Compile and Run

1. Build the Workspace

Build the workspace (if not already built):

colcon build --symlink-install
source install/setup.bash

2. Run and Visualize

You will need two sourced ROS 2 terminals and the Foxglove Studio application.

Terminal 1 - Start Foxglove Bridge

Launch the websocket bridge that Foxglove Studio uses to communicate with ROS 2:

ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765

Foxglove Studio GUI

  1. Open Foxglove Studio.

  2. Go to Open Connection → Foxglove WebSocket.

  3. Enter ws://localhost:8765 and click Open.

  4. Load the pre-configured layout from loam_scan_matcher/config/scan_matching_layout.json to visualize the following topics:

    • /features/edge_cloud (point cloud, colorized by intensity)

    • /features/planar_cloud (point cloud, colorized by intensity)

    • /scan_match/trajectory (path visualization)

    • /scan_match/odometry (pose and position plots)

  5. The layout includes:

    • 3D View (main): Point clouds, trajectory path, grid in map frame, and robot axes at base_link.

    • Transform Tree: Visualization of the TF hierarchy (map → base_link → velodyne).

    • Topic Graph: Node and topic connectivity.

    • Feature Widths Plot: Edge and planar cloud sizes over time.

    • Position Plot: X, Y, Z trajectory coordinates.

Terminal 2 - Launch the Scan Matching Pipeline

Once Foxglove Studio is connected, launch all nodes:

ros2 launch loam_scan_matcher scan_matching.launch.py

Monitor Pose Estimation

To verify that the scan matcher is successfully estimating pose (not stalled):

ros2 topic echo /scan_match/odometry --field pose.pose.position

Position values should change as the KITTI dataset plays through the sequence.

Troubleshooting

Pose Not Updating (Position Constant)

If /scan_match/odometry shows the same position repeatedly:

  1. Check that edge and planar clouds have sufficient points:

    ros2 topic echo /features/edge_cloud --field width
ros2 topic echo /features/planar_cloud --field width

  2. Verify ICP convergence by reducing icp_dist_thresh or increasing max_icp_iter in params.yaml.

  3. Check the TF tree:

    ros2 run tf2_tools view_frames

    The tree should show map → base_link.

TF Transform Errors in Foxglove

If Foxglove reports “Missing transform”, ensure:

  1. The frame_map and frame_base parameters match your layout’s frame IDs.

  2. The TF broadcaster is running (check /tf topic):

    ros2 topic list | grep tf

Noisy or Jumping Trajectories

If the estimated path jitters or jumps:

  1. Increase voxel_size to reduce noise sensitivity.

  2. Increase icp_dist_thresh to include more correspondences.

  3. Check that the feature extractor is producing balanced edge/planar distributions (see feature width plots).

Module