cartographer/mapping/internal/3d/local_trajectory_builder_3d.h

Overview

LocalTrajectoryBuilder3D wires up the 3D local SLAM stack — IMU-based pose extrapolation, range data accumulation and voxel filtering, two-resolution scan matching, and submap insertion — without loop closure. It is the internal per-trajectory engine behind 3D SLAM and produces MatchingResult objects that feed the pose graph.

Class: cartographer::mapping::LocalTrajectoryBuilder3D

Constructor

explicit LocalTrajectoryBuilder3D(
    const mapping::proto::LocalTrajectoryBuilderOptions3D& options,
    const std::vector<std::string>& expected_range_sensor_ids);

Initializes ActiveSubmaps3D, the MotionFilter, the real-time correlative scan matcher, the Ceres scan matcher, and a RangeDataCollator.

Result Types

struct InsertionResult {
  std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
  std::vector<std::shared_ptr<const mapping::Submap3D>> 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

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

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

  • AddImuData() — Lazily constructs the extrapolator from buffered IMU data and any initial_imu_data/initial_poses options; afterwards forwards IMU samples to it.

  • AddRangeData() — Returns a MatchingResult when accumulation completes, otherwise nullptr.

  • AddOdometryData() — Forwards to the extrapolator once initialized.

Local SLAM Pipeline

1. Synchronization and Accumulation

AddRangeData runs incoming scans through the RangeDataCollator. If use_intensities is set, the number of intensities must match the number of ranges. Accumulated scans are voxel filtered at 0.5 * voxel_filter_size before being appended to accumulated_point_cloud_origin_data_. Once num_accumulated_range_data scans have been collected, processing continues.

2. Pose Per Point and IMU Extrapolation

For every point across the accumulated scans, a timestamp is computed and any backwards-jumping timestamps are clamped. ExtrapolatePosesWithGravity produces one pose per hit time plus the current pose and gravity alignment. Points are transformed into the local frame and range-filtered:

  • Below min_range — dropped.

  • Within [min_range, max_range] — kept as returns (with intensity if enabled).

  • Beyond max_range — converted to misses cropped to max_range.

The returns are voxel filtered with voxel_filter_size to form filtered_range_data_in_tracking.

3. Two-Resolution Scan Matching

std::unique_ptr<transform::Rigid3d> ScanMatch(
    const transform::Rigid3d& pose_prediction,
    const sensor::PointCloud& low_resolution_point_cloud_in_tracking,
    const sensor::PointCloud& high_resolution_point_cloud_in_tracking);

AddAccumulatedRangeData applies the adaptive voxel filters to produce high- and low-resolution point clouds. Scan matching runs against the front submap:

  1. The pose prediction is expressed in the submap frame (submap.local_pose().inverse() * pose_prediction).

  2. If use_online_correlative_scan_matching, the RealTimeCorrelativeScanMatcher3D matches the high-resolution cloud against the submap’s high-resolution hybrid grid and refines initial_ceres_pose.

  3. CeresScanMatcher3D::Match optimizes against the high-resolution hybrid grid (plus the optional intensity hybrid grid) and the low-resolution hybrid grid simultaneously.

  4. The result is transformed back to the local frame and returned.

The matched pose is fed back into the extrapolator via AddPose.

4. Submap Insertion

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

If the MotionFilter decides the pose is different enough:

  1. A rotational scan matcher histogram is computed from the returns rotated into the gravity frame (RotationalScanMatcher::ComputeHistogram, size rotational_histogram_size).

  2. active_submaps_.InsertData inserts the range data with local_from_gravity_aligned rotation and the histogram.

  3. An InsertionResult is produced carrying the full TrajectoryNode::Data: time, gravity alignment, high/low-resolution point clouds, rotational histogram, and the pose estimate.

Metrics

RegisterMetrics exports latency, voxel-filter/scan-matcher/insert fractions of sensor time, real-time and CPU ratios, scan matcher scores/costs/residuals.

Configuration

From trajectory_builder_3d.lua:

TRAJECTORY_BUILDER_3D = {
  min_range = 1.,
  max_range = 60.,
  num_accumulated_range_data = 1,
  voxel_filter_size = 0.1,
  use_imu_data = true,
  use_online_correlative_scan_matching = false,
  use_intensities = false,
  high_resolution_adaptive_voxel_filter = { ... },
  low_resolution_adaptive_voxel_filter = { ... },
  rotational_histogram_size = 128,
  motion_filter = { ... },
  real_time_correlative_scan_matcher = { ... },
  ceres_scan_matcher = { ... },
  submaps = { ... },
  pose_extrapolator = { ... },
}

See Also