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

Overview

FastCorrelativeScanMatcher2D implements the branch-and-bound variant of the “Real-Time Correlative Scan Matching” algorithm (Olson). It trades precomputation time for very fast matching: a fixed grid is precomputed once into a multi-resolution stack of maximum-probability maps, after which candidate poses are scored against the map using a branch-and-bound search. Because the precomputation is tied to a single immutable Grid2D, it is primarily used in global/loop-closure matching rather than in online local SLAM.

Class: cartographer::mapping::scan_matching::FastCorrelativeScanMatcher2D

Construction

FastCorrelativeScanMatcher2D(
    const Grid2D& grid,
    const proto::FastCorrelativeScanMatcherOptions2D& options);

Stores the grid limits and builds a PrecomputationGridStack2D from the grid.

Matching Methods

bool Match(const transform::Rigid2d& initial_pose_estimate,
           const sensor::PointCloud& point_cloud, float min_score,
           float* score, transform::Rigid2d* pose_estimate) const;

bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score,
                     float* score, transform::Rigid2d* pose_estimate) const;

Both return true only if a pose with a score above min_score (strictly greater) exists. Match restricts the search to the configured linear_search_window and angular_search_window; MatchFullSubmap searches the entire submap (using a linear window large enough to cover the whole grid and a full 180-degree angular window) centered on the submap.

PrecomputationGrid2D

A grid whose cell (x0, y0) stores the maximum probability over the width x width region x0 <= x < x0 + width, y0 <= y < y0 + width. The maximum is computed in amortized O(1) per cell using a SlidingWindowMaximum (a deque of non-ascending maxima) applied first along rows and then along columns. Cell probabilities are quantized to 0..255 and mapped back to scores in [min_score, max_score] via ToScore.

class PrecomputationGrid2D {
 public:
  int GetValue(const Eigen::Array2i& xy_index) const;   // 0..255
  float ToScore(float value) const;                     // maps to [min, max]
};

PrecomputationGridStack2D

A stack of PrecomputationGrid2D levels. Level i uses a box width 1 << i. The top of the stack (level max_depth() = branch_and_bound_depth - 1) provides the coarsest score bound used to seed the search.

Branch-and-Bound Matching

MatchWithSearchParameters performs the search:

  1. Rotate the point cloud by the initial rotation and generate rotated_scans at discrete angular steps (GenerateRotatedScans from correlative_scan_matcher_2d.h).

  2. Discretize each rotated scan into DiscreteScan2D (integer cell indices) via DiscretizeScans.

  3. ShrinkToFit narrows the per-scan linear bounds so only feasible offsets remain.

  4. GenerateLowestResolutionCandidates enumerates candidate offsets at the coarsest resolution, and ScoreCandidates scores them against the coarsest precomputation grid, sorting best-first.

  5. BranchAndBound recursively subdivides the best candidates into higher-resolution offsets (2^depth partitions), pruning any branch whose bound is at or below the current best score, until depth 0 yields the best candidate.

Candidate2D BranchAndBound(
    const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    const std::vector<Candidate2D>& candidates,
    int candidate_depth, float min_score) const;

The winning Candidate2D’s linear and angular offsets are added to the initial pose estimate to produce the final pose_estimate, and its score is returned via the out-parameter.

Supporting Types (from correlative_scan_matcher_2d.h)

typedef std::vector<Eigen::Array2i> DiscreteScan2D;

struct SearchParameters {
  struct LinearBounds { int min_x, max_x, min_y, max_y; };
  int num_angular_perturbations;
  double angular_perturbation_step_size;
  double resolution;
  int num_scans;
  std::vector<LinearBounds> linear_bounds;  // per rotated scan
  void ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
                   const CellLimits& cell_limits);
};

struct Candidate2D {
  int scan_index;
  int x_index_offset, y_index_offset;
  double x, y, orientation;  // relative to the initial pose
  float score;               // higher is better
};

Options

proto::FastCorrelativeScanMatcherOptions2D:

Field

Type

Meaning

linear_search_window

double

Linear search window in meters

angular_search_window

double

Angular search window in radians

branch_and_bound_depth

int

Number of precomputation levels (2D search depth)

Configuration

From pose_graph.lua:

fast_correlative_scan_matcher = {
  linear_search_window = 7.,
  angular_search_window = 3. / 180. * math.pi,
  branch_and_bound_depth = 7,
}

The factory CreateFastCorrelativeScanMatcherOptions2D() reads these three keys.

Role in the SLAM Pipeline

  • Loop closure (constraint builder): ConstraintBuilder2D constructs a FastCorrelativeScanMatcher2D per submap to propose candidate loop-closure poses that exceed min_score, which are then refined with CeresScanMatcher2D.

  • Global localization: MatchFullSubmap is used to localize a trajectory against a full submap without a prior pose.

See Also