Graph SLAM (pose graph optimization)

Graph SLAM (pose graph optimization) – definition

Graph SLAM is a simultaneous localization and mapping method that represents a mobile robot trajectory as a graph of spatial constraints. Each vertex usually represents a robot pose at a specific time. Each edge usually represents a relative-pose measurement between two vertices, while global measurements can be represented as prior or unary constraints. Measurements can come from wheel odometry, IMU integration, LiDAR scan matching, visual odometry, GNSS, or loop-closure detection.

In mobile robotics, pose graph optimization estimates the set of robot poses that best satisfies all available constraints. It is commonly used after, or together with, front-end scan matching and place-recognition processes. On a wheeled UGV such as Leo Rover or Raph Rover, Graph SLAM can correct accumulated odometry drift when the robot revisits a previously mapped area.

The optimization problem for pairwise relative-pose constraints is typically written as:

x* = arg min Σᵢⱼ eᵢⱼ(xᵢ, xⱼ, zᵢⱼ)ᵀ Ωᵢⱼ eᵢⱼ(xᵢ, xⱼ, zᵢⱼ)

Here, xᵢ and xⱼ are robot poses, zᵢⱼ is a relative measurement, eᵢⱼ is the residual error, and Ωᵢⱼ is the information matrix representing measurement confidence. The result is an optimized trajectory and, depending on the SLAM implementation, an updated occupancy grid, point cloud map, or submap collection.

Pose graphs in UGV localization and mapping

A pose graph separates the estimation problem from the hardware interfaces that produce measurements. This is useful on modular UGV platforms because the sensor configuration may change between deployments. A Leo Rover can use a 2D LiDAR and wheel odometry for indoor mapping. A Raph Rover with a larger payload capacity can carry additional sensors, such as a 3D LiDAR, stereo camera, GNSS receiver, or RTK-capable positioning system.

Typical graph constraints include the following:

Odometry constraints – relative motion estimated from wheel encoders, visual odometry, or fused inertial data.
Scan-matching constraints – spatial alignment between consecutive LiDAR scans or local submaps.
Loop-closure constraints – constraints added when the robot recognizes a previously visited location.
Global-position constraints – position measurements from GNSS, including RTK-enabled systems, commonly used outdoors.

Wheel odometry alone is not sufficient for globally consistent mapping. Differential-drive vehicles accumulate position error because wheel slip, unequal wheel contact, terrain irregularities, and encoder uncertainty affect the motion estimate. Graph optimization reduces this error when independent constraints, especially loop closures, are available.

Graph SLAM coordinate frames in ROS 2

ROS 2 navigation systems use a transform tree to express robot motion and map coordinates. The conventional frame arrangement for mobile robots is described by REP 105 – Coordinate Frames for Mobile Platforms. A Graph SLAM system normally publishes a transform between the global map frame and the local odometry frame.

Frame	Purpose in a Graph SLAM system
`map`	Global frame corrected by loop closures and graph optimization.
`odom`	Locally continuous frame based on wheel odometry or state estimation.
`base_link`	Robot body frame attached to the UGV chassis.
`laser` or sensor frame	Frame of a LiDAR, camera, IMU, or GNSS antenna, typically connected through static transforms.

The transform from odom to base_link should be generated by the robot odometry or state-estimation system. The SLAM component typically publishes map → odom. This division prevents discrete global corrections from directly breaking local control and odometry continuity.

Relevant ROS 2 interfaces commonly include sensor_msgs/msg/LaserScan, sensor_msgs/msg/Imu, nav_msgs/msg/Odometry, nav_msgs/msg/OccupancyGrid, and transforms published on /tf and /tf_static. Their message definitions are maintained in the official ROS 2 documentation.

Graph SLAM implementations for ROS 2

Two commonly used ROS-compatible implementations are Cartographer and slam_toolbox. They use different front-end and back-end designs, but both can construct pose graphs and perform loop-closure optimization.

Cartographer supports 2D and 3D SLAM workflows based on local trajectory building, submaps, scan matching, and global pose-graph optimization. Its configuration accepts range data, IMU data, odometry, and optional fixed-frame pose information. Source: Cartographer ROS documentation.
slam_toolbox is a ROS 2 SLAM package focused on 2D occupancy-grid mapping. It supports online synchronous, online asynchronous, localization, and serialized-map workflows. Source: slam_toolbox ROS 2 package documentation.

For a Raspberry Pi-based Leo Rover, a 2D LiDAR configuration is generally less computationally demanding than dense 3D LiDAR or multi-camera SLAM. The selected algorithm must still match the available CPU, memory, sensor drivers, and required map size. ROS 2 support, platform-specific interfaces, and configuration details should be verified in Leo Rover technical documentation.

Example ROS 2 configuration inputs

A Graph SLAM node requires correct topic names, frame identifiers, and transform availability. The following simplified YAML fragment illustrates configuration inputs used by a 2D SLAM workflow. Exact parameter names depend on the selected package and ROS 2 distribution.

slam_node:
  ros__parameters:
    map_frame: map
    odom_frame: odom
    base_frame: base_link
    scan_topic: /scan
    use_odometry: true
    use_imu: false
    mode: mapping

Before running mapping, verify that the transform chain is complete and that sensor timestamps use the same clock source. A basic ROS 2 inspection workflow can use the following commands:

ros2 topic list
ros2 topic echo /scan --once
ros2 run tf2_ros tf2_echo odom base_link
ros2 run tf2_tools view_frames

Limitations and validation criteria

Graph SLAM is not a substitute for sensor calibration, reliable odometry, or correct frame transforms. Incorrect LiDAR mounting transforms, inconsistent timestamps, unmodeled wheel slip, and repeated geometric structures can create false loop closures or distorted maps. Outdoor deployments can also be affected by vegetation, moving people, changing illumination, dust, and limited GNSS visibility.

Validation should examine trajectory consistency, loop-closure plausibility, map alignment with measured geometry, transform stability, and navigation behavior after map updates. On Leo Rover and Raph Rover, tests should begin at low speed in a controlled route with known landmarks before the optimized map is used by a ROS 2 navigation stack.

Graph SLAM (pose graph optimization)