# 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

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

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

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

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

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

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

- [CeresScanMatcher3D](scan_matching/ceres_scan_matcher_3d.md) — Nonlinear refinement stage
- `RealTimeCorrelativeScanMatcher3D` — Optional online coarse matcher (3D variant of the [2D real-time matcher](../2d/scan_matching/real_time_correlative_scan_matcher_2d.md))
- [ImuBasedPoseExtrapolator](../imu_based_pose_extrapolator.md) — Used when `use_imu_based` is set
- [MotionFilter](../motion_filter.md) — Drops similar poses before insertion
- [LocalTrajectoryBuilder2D](../2d/local_trajectory_builder_2d.md) — 2D counterpart