pose_graph.lua¶

Overview¶

Configuration for global pose graph optimization. Controls loop closure detection, constraint building, and Ceres-based bundle adjustment.

Configuration¶

POSE_GRAPH = {
  optimize_every_n_nodes = 90,
  constraint_builder = {
    sampling_ratio = 0.3,
    max_constraint_distance = 15.,
    min_score = 0.55,
    global_localization_min_score = 0.6,
    loop_closure_translation_weight = 1.1e4,
    loop_closure_rotation_weight = 1e5,
    log_matches = true,
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,
      angular_search_window = math.rad(30.),
      branch_and_bound_depth = 7,
    },
    ceres_scan_matcher = {
      occupied_space_weight = 20.,
      translation_weight = 10.,
      rotation_weight = 1.,
      ceres_solver_options = {
        use_nonmonotonic_steps = true,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
    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.rad(15.),
    },
    ceres_scan_matcher_3d = {
      occupied_space_weight_0 = 5.,
      occupied_space_weight_1 = 30.,
      translation_weight = 10.,
      rotation_weight = 1.,
      only_optimize_yaw = false,
      ceres_solver_options = {
        use_nonmonotonic_steps = false,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
  },
  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,
  optimization_problem = {
    huber_scale = 1e1,
    acceleration_weight = 1.1e2,
    rotation_weight = 1.6e4,
    local_slam_pose_translation_weight = 1e5,
    local_slam_pose_rotation_weight = 1e5,
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,
    fixed_frame_pose_use_tolerant_loss = false,
    fixed_frame_pose_tolerant_loss_param_a = 1,
    fixed_frame_pose_tolerant_loss_param_b = 1,
    log_solver_summary = false,
    use_online_imu_extrinsics_in_3d = true,
    fix_z_in_3d = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
  },
  max_num_final_iterations = 200,
  global_sampling_ratio = 0.003,
  log_residual_histograms = true,
  global_constraint_search_after_n_seconds = 10.,
}

Parameter Reference¶

Optimization Schedule¶

Parameter	Description
`optimize_every_n_nodes`	Run optimization every N new nodes
`max_num_final_iterations`	Iterations for final optimization
`global_sampling_ratio`	Fraction of nodes for global search
`global_constraint_search_after_n_seconds`	Delay before global loop closure

Constraint Builder (Loop Closure Detection)¶

Parameter	Description
`sampling_ratio`	Fraction of nodes to check for loop closure
`max_constraint_distance`	Max distance for constraint (m)
`min_score`	Min scan match score for constraint
`global_localization_min_score`	Min score for global localization
`loop_closure_translation_weight`	Translation weight in constraint
`loop_closure_rotation_weight`	Rotation weight in constraint
`log_matches`	Log detected loop closures

2D Fast Correlative Scan Matcher¶

Coarse search for loop closure candidates:

linear_search_window — XY search radius (m)
angular_search_window — Yaw search range (rad)
branch_and_bound_depth — Search tree depth

2D Ceres Scan Matcher¶

Refines loop closure constraints:

occupied_space_weight — Occupancy cost weight
translation_weight / rotation_weight — Prior weights
ceres_solver_options — Solver config

3D Fast Correlative Scan Matcher¶

Branch-and-bound search in SE(3):

branch_and_bound_depth — Tree depth
full_resolution_depth — Depth at full resolution
min_rotational_score / min_low_resolution_score — Pruning thresholds
linear_xy_search_window / linear_z_search_window — Translation bounds
angular_search_window — Rotation bounds

3D Ceres Scan Matcher¶

Refines 3D loop closures:

occupied_space_weight_0 / occupied_space_weight_1 — High/low res weights
only_optimize_yaw — Constrain to yaw-only
ceres_solver_options — Solver config

Global Optimization¶

Matcher Weights¶

Parameter	Description
`matcher_translation_weight`	Scan match translation cost weight
`matcher_rotation_weight`	Scan match rotation cost weight

Optimization Problem Cost Functions¶

Parameter	Cost Function	Description
`huber_scale`	All	Huber loss scale
`acceleration_weight`	Acceleration	IMU acceleration prior
`rotation_weight`	Rotation	Rotation change prior
`local_slam_pose_translation_weight`	Local SLAM	Local pose prior (translation)
`local_slam_pose_rotation_weight`	Local SLAM	Local pose prior (rotation)
`odometry_translation_weight`	Odometry	Odometry prior (translation)
`odometry_rotation_weight`	Odometry	Odometry prior (rotation)
`fixed_frame_pose_translation_weight`	Fixed Frame	GPS/external pose (translation)
`fixed_frame_pose_rotation_weight`	Fixed Frame	GPS/external pose (rotation)

Fixed Frame Pose Options¶

fixed_frame_pose_use_tolerant_loss — Use tolerant loss for outliers
fixed_frame_pose_tolerant_loss_param_a/b — Tolerant loss params

3D-Specific¶

use_online_imu_extrinsics_in_3d — Optimize IMU extrinsics online
fix_z_in_3d — Constrain Z axis (for planar motion)

Ceres Solver¶

use_nonmonotonic_steps — Allow non-monotonic steps
max_num_iterations — Max solver iterations
num_threads — Parallel solver threads
log_solver_summary — Log Ceres summary

Tuning Guide¶

For Better Loop Closure Detection¶

Increase sampling_ratio (more candidates)
Increase max_constraint_distance
Decrease min_score (more permissive)
Increase fast_correlative_scan_matcher.linear_search_window

For More Accurate Global Map¶

Increase optimization_problem.local_slam_pose_*_weight
Increase matcher_translation_weight / matcher_rotation_weight
Increase max_num_final_iterations
Enable log_solver_summary for debugging

For GPS/External Pose Fusion¶

Set fixed_frame_pose_*_weight appropriately
Enable fixed_frame_pose_use_tolerant_loss for robustness
Tune fixed_frame_pose_tolerant_loss_param_a/b

For 3D with IMU¶

Enable use_online_imu_extrinsics_in_3d
Ensure pose_extrapolator.use_imu_based = true in trajectory builder
Set appropriate acceleration_weight and rotation_weight

For Large-Scale Maps¶

Decrease global_sampling_ratio (less frequent global search)
Increase global_constraint_search_after_n_seconds
Use more solver threads (num_threads)