constraint_builder.h¶

Overview¶

constraint_builder.h provides the shared options factory for the pose graph’s loop-closure machinery. The actual constraint computation is performed asynchronously by the concrete classes in the same directory: ConstraintBuilder2D and ConstraintBuilder3D. Both are thread-safe classes that schedule background work on a ThreadPoolInterface to find constraints (loop closures) between submaps and nodes.

Options Factory¶

proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
    common::LuaParameterDictionary* parameter_dictionary);

Reads sampling_ratio, max_constraint_distance, min_score, global_localization_min_score, loop_closure_translation_weight, loop_closure_rotation_weight, and log_matches, plus the nested fast_correlative_scan_matcher and ceres_scan_matcher dictionaries (2D) and fast_correlative_scan_matcher_3d and ceres_scan_matcher_3d dictionaries (3D).

ConstraintBuilder2D / ConstraintBuilder3D¶

Both classes expose the same interface. The 2D variant matches Submap2D grids with FastCorrelativeScanMatcher2D + CeresScanMatcher2D; the 3D variant matches Submap3D hybrid grids with FastCorrelativeScanMatcher3D + CeresScanMatcher3D.

Construction¶

ConstraintBuilder2D(const proto::ConstraintBuilderOptions& options,
                    common::ThreadPoolInterface* thread_pool);
ConstraintBuilder3D(const proto::ConstraintBuilderOptions& options,
                    common::ThreadPoolInterface* thread_pool);

Public API¶

using Constraint = PoseGraphInterface::Constraint;
using Result = std::vector<Constraint>;

// Schedule a constraint search between a submap and a node, using an
// 'initial_relative_pose' (relative to the submap) to seed the search.
void MaybeAddConstraint(const SubmapId& submap_id, const Submap2D* submap,
                        const NodeId& node_id,
                        const TrajectoryNode::Data* const constant_data,
                        const transform::Rigid2d& initial_relative_pose);

// Schedule a full-submap (global localization) match, without a pose prior.
void MaybeAddGlobalConstraint(const SubmapId& submap_id, const Submap2D* submap,
                              const NodeId& node_id,
                              const TrajectoryNode::Data* const constant_data);

void NotifyEndOfNode();

void WhenDone(const std::function<void(const Result&)>& callback);

int GetNumFinishedNodes();
void DeleteScanMatcher(const SubmapId& submap_id);
static void RegisterMetrics(metrics::FamilyFactory* family_factory);

Usage Pattern¶

The intended interleaving is: call MaybeAddConstraint / MaybeAddGlobalConstraint and NotifyEndOfNode any number of times, then call WhenDone once. When all background computations finish, the callback is executed (in the thread pool) with the resulting constraints, after which another cycle can begin.

The pointees of submap and constant_data must remain valid until all computations are finished.

Background Pipeline¶

Per submap, a SubmapScanMatcher is built lazily on a background thread. For 2D it holds the submap Grid2D and a FastCorrelativeScanMatcher2D; for 3D it holds the hybrid grids and a FastCorrelativeScanMatcher3D. Construction tasks must complete before the grid/matcher is accessed.
ComputeConstraint (running in the thread pool) first filters candidates: a FixedRatioSampler applies sampling_ratio, and nodes farther than max_constraint_distance from the submap are skipped.
The fast correlative matcher proposes candidate poses above min_score (or global_localization_min_score for MaybeAddGlobalConstraint), and the Ceres matcher refines the best candidate.
If the refined match scores above min_score, a Constraint is added to the results, weighted by loop_closure_translation_weight and loop_closure_rotation_weight.
NotifyEndOfNode increments the finished-node counter; WhenDone fires once all scheduled work is complete.

Options¶

proto::ConstraintBuilderOptions (from pose_graph/constraint_builder_options.proto):

Field	Type	Meaning
`sampling_ratio`	double	Fraction of nodes sampled per submap for constraint search
`max_constraint_distance`	double	Maximum node-to-submap distance considered
`min_score`	double	Minimum match score to accept a constraint
`global_localization_min_score`	double	Minimum score for full-submap matches
`loop_closure_translation_weight`	double	Translation weight of accepted constraints
`loop_closure_rotation_weight`	double	Rotation weight of accepted constraints
`log_matches`	bool	Log accepted matches
`fast_correlative_scan_matcher` / `_3d`	message	Options for the candidate matcher
`ceres_scan_matcher` / `_3d`	message	Options for the refinement matcher

Configuration¶

From pose_graph.lua:

constraint_builder = {
  sampling_ratio = 0.3,
  max_constraint_distance = 15.,
  min_score = 0.55,
  global_localization_min_score = 0.66,
  loop_closure_translation_weight = 1.1e4,
  loop_closure_rotation_weight = 1e5,
  log_matches = true,
  fast_correlative_scan_matcher = { ... },
  ceres_scan_matcher = { ... },
  fast_correlative_scan_matcher_3d = { ... },
  ceres_scan_matcher_3d = { ... },
}

Role in the SLAM Pipeline¶

The pose graph periodically triggers constraint building as submaps are finished and new nodes are added. The constraints produced by the constraint builder are fed into the optimization problem (see optimization_problem_interface.md), which minimizes their residuals to update global poses.