cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h

Overview

FastCorrelativeScanMatcher3D implements a 3D branch-and-bound scan matcher analogous to FastCorrelativeScanMatcher2D. It matches a node’s stored point cloud against a submap’s hybrid grid and returns a Result with the match score, pose estimate, rotational score, and low-resolution score. It is used by the 3D constraint builder for loop closure.

Class: cartographer::mapping::scan_matching::FastCorrelativeScanMatcher3D

Construction

FastCorrelativeScanMatcher3D(
    const HybridGrid& hybrid_grid,
    const HybridGrid* low_resolution_hybrid_grid,
    const Eigen::VectorXf* rotational_scan_matcher_histogram,
    const proto::FastCorrelativeScanMatcherOptions3D& options);

Stores the resolution and grid size, builds the PrecomputationGridStack3D from the high-resolution grid, and keeps a pointer to the low-resolution grid plus a RotationalScanMatcher built from the reference histogram.

Result

struct Result {
  float score;
  transform::Rigid3d pose_estimate;
  float rotational_score;
  float low_resolution_score;
};

Matching Methods

std::unique_ptr<Result> Match(const transform::Rigid3d& global_node_pose,
                              const transform::Rigid3d& global_submap_pose,
                              const TrajectoryNode::Data& constant_data,
                              float min_score) const;

std::unique_ptr<Result> MatchFullSubmap(
    const Eigen::Quaterniond& global_node_rotation,
    const Eigen::Quaterniond& global_submap_rotation,
    const TrajectoryNode::Data& constant_data, float min_score) const;

  • Match uses the configured linear_xy_search_window, linear_z_search_window, and angular_search_window (converted to voxels/radians), matching the node’s high-resolution point cloud against the submap.

  • MatchFullSubmap searches the whole submap: the linear window covers (grid_size + 1) / 2 + max_point_distance / resolution voxels, the angular window is full 180 degrees, and the search is seeded only by rotations (expecting approximately gravity-aligned poses).

Both return nullptr unless a score strictly above min_score is achievable.

Matching Pipeline

  1. Precomputation stack. PrecomputationGridStack3D builds one PrecomputationGrid3D per depth. Depth 0 is the full-resolution grid; deeper levels are built by block-max precomputation, optionally at half resolution once full_resolution_depth is exceeded. Each cell of a level stores the maximum occupancy probability over a 2^depth cube.

  2. Rotational scan matching. GenerateDiscreteScans computes an angular step size from the maximum scan range, sweeps yaw angles inside the angular window, and scores each yaw with the RotationalScanMatcher against the node’s rotational histogram. Rotations whose score falls below min_rotational_score are discarded. Each surviving yaw produces a DiscreteScan3D (a pose plus per-depth discretized cell indices).

  3. Low-resolution candidates. GenerateLowestResolutionCandidates enumerates offsets on a grid of step 2^max_depth for each discrete scan. ComputeLowestResolutionCandidates scores them at the coarsest level and sorts best-first.

  4. Branch and bound. BranchAndBound recursively refines candidate offsets into 2^depth sub-blocks at each level, pruning branches whose score bound cannot beat min_score. At depth 0 it additionally evaluates each surviving candidate with the low-resolution matcher (via the MatchingFunction), requiring min_low_resolution_score to accept the match.

struct SearchParameters {
  const int linear_xy_window_size;   // voxels
  const int linear_z_window_size;    // voxels
  const double angular_search_window; // radians
  const MatchingFunction* const low_resolution_matcher;
};

Options

proto::FastCorrelativeScanMatcherOptions3D:

Field

Type

Meaning

branch_and_bound_depth

int

Number of search levels in the precomputation stack

full_resolution_depth

int

Number of levels kept at full resolution

min_rotational_score

double

Minimum score for a yaw candidate

min_low_resolution_score

double

Minimum score for the low-resolution matcher

linear_xy_search_window

double

Linear search window in x/y (meters)

linear_z_search_window

double

Linear search window in z (meters)

angular_search_window

double

Angular search window in radians

Configuration

From pose_graph.lua:

fast_correlative_scan_matcher_3d = {
  branch_and_bound_depth = 8,
  full_resolution_depth = 3,
  min_rotational_score = 0.77,
  min_low_resolution_score = 0.55,
  linear_xy_search_window = 5.,
  linear_z_search_window = 1.,
  angular_search_window = math.pi / 8.,
}

CreateFastCorrelativeScanMatcherOptions3D() reads all seven keys.

Role in the SLAM Pipeline

ConstraintBuilder3D constructs one matcher per submap and uses Match to propose loop-closure candidates from a node’s TrajectoryNode::Data. MatchFullSubmap supports global localization from rotation-only seeds. Accepted candidates are refined by CeresScanMatcher3D.

See Also