cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h

Overview

CeresScanMatcher2D aligns a scan with an existing 2D grid map using Ceres nonlinear least-squares optimization. It is the refinement stage of 2D scan matching: given an initial pose estimate (typically from the pose prediction or the online correlative matcher), it produces a refined Rigid2d pose estimate plus the Ceres solver summary.

Class: cartographer::mapping::scan_matching::CeresScanMatcher2D

Construction

explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options);

Builds the Ceres solver options from the proto (forcing ceres::DENSE_QR as the linear solver type).

Match

void Match(const Eigen::Vector2d& target_translation,
           const transform::Rigid2d& initial_pose_estimate,
           const sensor::PointCloud& point_cloud, const Grid2D& grid,
           transform::Rigid2d* pose_estimate,
           ceres::Solver::Summary* summary) const;

The optimization state is (x, y, theta), seeded from initial_pose_estimate. Three classes of residual blocks are added:

  1. Map-matching term (weighted by occupied_space_weight / sqrt(point_cloud.size())):

    • For GridType::PROBABILITY_GRID, an OccupiedSpaceCostFunction2D scores points against the ProbabilityGrid.

    • For GridType::TSDF, a TSDFMatchCostFunction2D scores points against the TSDF2D using truncated signed distances.

  2. Translation prior (TranslationDeltaCostFunctor2D, weighted by translation_weight) penalizing deviation from target_translation.

  3. Rotation prior (RotationDeltaCostFunctor2D, weighted by rotation_weight) penalizing rotation away from the initial angle.

After ceres::Solve, pose_estimate is reconstructed from the optimized (x, y, theta) triple.

Options

proto::CeresScanMatcherOptions2D (from scan_matching/ceres_scan_matcher_options_2d.proto):

Field

Type

Meaning

occupied_space_weight

double

Weight of the grid-matching cost term

translation_weight

double

Weight penalizing translation from the target

rotation_weight

double

Weight penalizing rotation from the initial angle

ceres_solver_options

CommonSolverOptions

Ceres solver configuration (e.g. iteration count, trust region strategy)

Configuration

From trajectory_builder_2d.lua:

ceres_scan_matcher = {
  occupied_space_weight = 1.,
  translation_weight = 10.,
  rotation_weight = 40.,
  ceres_solver_options = {
    use_nonmonotonic_steps = false,
    max_num_iterations = 20,
    num_threads = 1,
  },
}

The Lua factory CreateCeresScanMatcherOptions2D() reads occupied_space_weight, translation_weight, rotation_weight, and the ceres_solver_options dictionary.

Role in the SLAM Pipeline

In LocalTrajectoryBuilder2D::ScanMatch, this matcher runs last — after the online real-time correlative matcher has produced a refined initial_ceres_pose — to obtain a sub-pixel-accurate pose. The same CeresScanMatcher2D is reused by the 2D constraint builder during loop closure.

See Also