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

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

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

PointCloudAndHybridGridsPointers

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

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:

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