# cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h

## Overview

`CeresScanMatcher3D` aligns scans with an existing 3D map using Ceres nonlinear least-squares optimization. It is the refinement stage of 3D scan matching: given an initial pose estimate, it optimizes a full `Rigid3d` pose against one or more `HybridGrid` maps (high-resolution, low-resolution, and optionally intensity) using auto-differentiable cost functions.

## Class: `cartographer::mapping::scan_matching::CeresScanMatcher3D`

### Construction

```cpp
explicit CeresScanMatcher3D(const proto::CeresScanMatcherOptions3D& options);
```

Builds the Ceres solver options from the proto and forces `ceres::DENSE_QR` as the linear solver.

### PointCloudAndHybridGridsPointers

```cpp
struct PointCloudAndHybridGridsPointers {
  const sensor::PointCloud* point_cloud;
  const HybridGrid* hybrid_grid;
  const IntensityHybridGrid* intensity_hybrid_grid;  // optional
};
```

Describes one point cloud matched against one (or optionally two) grids. In `LocalTrajectoryBuilder3D` two of these are passed: the high-resolution cloud against the high-resolution grid (with the intensity grid when `use_intensities`), and the low-resolution cloud against the low-resolution grid.

### Match

```cpp
void Match(const Eigen::Vector3d& target_translation,
           const transform::Rigid3d& initial_pose_estimate,
           const std::vector<PointCloudAndHybridGridsPointers>&
               point_clouds_and_hybrid_grids,
           transform::Rigid3d* pose_estimate,
           ceres::Solver::Summary* summary) const;
```

The state is a `CeresPose` (translation + quaternion rotation). The rotation parameterization is chosen by `only_optimize_yaw`:
- `true` — `YawOnlyQuaternionOperations` / `YawOnlyQuaternionPlus` manifold, which fixes roll and pitch.
- `false` — a full quaternion manifold.

Residual blocks:

1. **Occupied-space term per cloud** — `OccupiedSpaceCostFunction3D` for each entry, weighted by `occupied_space_weight(i) / sqrt(point_cloud.size())`. There must be exactly as many `occupied_space_weight` entries as clouds.
2. **Intensity term** (optional per cloud) — `IntensityCostFunction3D` weighted by `intensity_cost_function_options(i).weight / sqrt(size)` with `intensity_threshold` and a `ceres::HuberLoss` with `huber_scale`. Only added when `intensity_hybrid_grid` is present.
3. **Translation prior** — `TranslationDeltaCostFunctor3D` weighted by `translation_weight` against `target_translation`.
4. **Rotation prior** — `RotationDeltaCostFunctor3D` weighted by `rotation_weight` against the initial rotation.

`ceres::Solve` produces the optimized pose, which is returned via `pose_estimate` (a `Rigid3d`).

## Options

`proto::CeresScanMatcherOptions3D`:

| Field | Type | Meaning |
|-------|------|---------|
| `occupied_space_weight` | repeated double | One weight per matched point cloud |
| `intensity_cost_function_options` | repeated | Per-cloud `{weight, huber_scale, intensity_threshold}` |
| `translation_weight` | double | Penalty on translation from the target |
| `rotation_weight` | double | Penalty on rotation from the initial estimate |
| `only_optimize_yaw` | bool | Fix roll/pitch (yaw-only manifold) |
| `ceres_solver_options` | CommonSolverOptions | Ceres solver configuration |

## Configuration

From `trajectory_builder_3d.lua`:

```lua
ceres_scan_matcher = {
  occupied_space_weight_0 = 1.,
  occupied_space_weight_1 = 1.,
  translation_weight = 10.,
  rotation_weight = 1.,
  only_optimize_yaw = false,
  ceres_solver_options = { ... },
}
```

`CreateCeresScanMatcherOptions3D()` reads an arbitrary number of `occupied_space_weight_<i>` and `intensity_cost_function_options_<i>` keys (until a missing index), then the scalar weights and solver options.

## Role in the SLAM Pipeline

`LocalTrajectoryBuilder3D::ScanMatch` calls this matcher after the optional online correlative stage to refine the pose in the submap frame. The same matcher is reused by the 3D constraint builder for loop closure refinement.

## See Also

- `RealTimeCorrelativeScanMatcher3D` — Coarse matcher before Ceres (3D variant of the [2D real-time matcher](../../2d/scan_matching/real_time_correlative_scan_matcher_2d.md))
- [FastCorrelativeScanMatcher3D](fast_correlative_scan_matcher_3d.md) — Loop-closure candidate matcher
- [LocalTrajectoryBuilder3D](../local_trajectory_builder_3d.md) — Online caller
- [CeresScanMatcher2D](../../2d/scan_matching/ceres_scan_matcher_2d.md) — 2D counterpart