# 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

```cpp
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

```cpp
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

```cpp
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

```cpp
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

```cpp
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

```cpp
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`:

```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

- [CeresScanMatcher2D](scan_matching/ceres_scan_matcher_2d.md) — Nonlinear refinement stage
- [RealTimeCorrelativeScanMatcher2D](scan_matching/real_time_correlative_scan_matcher_2d.md) — Optional online coarse matcher
- [MotionFilter](../motion_filter.md) — Drops similar poses before insertion
- [LocalTrajectoryBuilder3D](../3d/local_trajectory_builder_3d.md) — 3D counterpart