cartographer/mapping/internal/2d/local_trajectory_builder_2d.h

Overview

LocalTrajectoryBuilder2D wires up the 2D local SLAM stack — pose extrapolation, range data accumulation and filtering, scan matching, and submap insertion — without loop closure. It is the internal per-trajectory engine behind the public TrajectoryBuilderInterface for 2D SLAM and produces MatchingResult objects that feed the pose graph.

Class: cartographer::mapping::LocalTrajectoryBuilder2D

Constructor

LocalTrajectoryBuilder2D(
    const proto::LocalTrajectoryBuilderOptions2D& options,
    const std::vector<std::string>& expected_range_sensor_ids);

Initializes the ActiveSubmaps2D, MotionFilter, real-time correlative scan matcher, Ceres scan matcher, and a RangeDataCollator for the expected range sensor IDs.

Result Types

struct InsertionResult {
  std::shared_ptr<const TrajectoryNode::Data> constant_data;
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
};

struct MatchingResult {
  common::Time time;
  transform::Rigid3d local_pose;
  sensor::RangeData range_data_in_local;
  // 'nullptr' if dropped by the motion filter.
  std::unique_ptr<const InsertionResult> insertion_result;
};

Public Methods

std::unique_ptr<MatchingResult> AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& range_data);
void AddImuData(const sensor::ImuData& imu_data);
void AddOdometryData(const sensor::OdometryData& odometry_data);

static void RegisterMetrics(metrics::FamilyFactory* family_factory);

  • AddRangeData() — Returns a MatchingResult when range data accumulation completes, otherwise nullptr. TimedPointCloudData::time is the time of the last point; ranges carry per-point relative times.

  • AddImuData() — Only valid when use_imu_data is enabled; feeds the extrapolator.

  • AddOdometryData() — Feeds the extrapolator once it is initialized.

  • RegisterMetrics() — Exposes latency, real-time ratio, CPU ratio, scan matcher scores/costs/residuals gauges and histograms.

Local SLAM Pipeline

1. Synchronization and Accumulation

AddRangeData first runs data through the RangeDataCollator, which buffers single-sensor scans until all expected range sensors contribute data at the same timestamp. It then extrapolates a pose for every point in the synchronized scan using the PoseExtrapolator, so points acquired during motion are transformed into the local frame with per-point poses.

Each point is range-filtered:

  • Points below min_range are dropped.

  • Points within [min_range, max_range] become hits (returns).

  • Points beyond max_range are turned into misses projected to missing_data_ray_length.

Data is accumulated until num_accumulated_range_data scans have been merged, then processed in AddAccumulatedRangeData.

2. Gravity Alignment and Filtering

sensor::RangeData TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const;

The accumulated range data is transformed into a gravity-aligned frame, cropped in z to [min_z, max_z], and voxel filtered with voxel_filter_size. A gravity-aligned pose prediction is computed by projecting the extrapolated pose and removing gravity alignment.

3. Scan Matching

ScanMatch() matches filtered_gravity_aligned_point_cloud against the front submap:

  1. If use_online_correlative_scan_matching is set, the RealTimeCorrelativeScanMatcher2D refines the pose prediction into an initial_ceres_pose and its score is recorded.

  2. CeresScanMatcher2D::Match is then run with the target translation from the prediction, producing the final pose_observation (a Rigid2d).

  3. Residual distance and angle versus the prediction are recorded as metrics.

The resulting 2D pose is embedded in 3D and combined with gravity alignment to form the final pose_estimate, which is fed back into the extrapolator via AddPose.

4. Submap Insertion

std::unique_ptr<InsertionResult> InsertIntoSubmap(
    common::Time time, const sensor::RangeData& range_data_in_local,
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud,
    const transform::Rigid3d& pose_estimate,
    const Eigen::Quaterniond& gravity_alignment);

The MotionFilter decides whether the pose moved enough to warrant insertion. If so, the range data is inserted into the active submaps and an InsertionResult is produced carrying the TrajectoryNode::Data (time, gravity alignment, filtered gravity-aligned point cloud, pose estimate) and the affected submaps. The 3D-only fields (high_resolution_point_cloud, low_resolution_point_cloud, rotational_scan_matcher_histogram) are empty in 2D.

Extrapolator Initialization

void InitializeExtrapolator(common::Time time);

The extrapolator is a constant-velocity PoseExtrapolator (the IMU-based variant is not used in 2D). If no IMU is used, it is initialized lazily on the first range data; otherwise it is initialized by the first IMU message.

Configuration

From trajectory_builder_2d.lua:

TRAJECTORY_BUILDER_2D = {
  min_range = 1.,
  max_range = 60.,
  min_z = -1.4,
  max_z = 1.6,
  missing_data_ray_length = 5.,
  num_accumulated_range_data = 1,
  voxel_filter_size = 0.025,
  use_imu_data = true,
  use_online_correlative_scan_matching = false,
  adaptive_voxel_filter = { ... },
  motion_filter = { ... },
  real_time_correlative_scan_matcher = { ... },
  ceres_scan_matcher = { ... },
  submaps = { ... },
  pose_extrapolator = { ... },
}

See Also