System Architecture Overview

The framework divides its operational pipeline into two distinct layers: the local frontend and the global backend. The frontend continuously ingests sensor streams to compute relative frame-to-frame transformations, constructing localized maps known as submaps while tracking the scanner's immediate pose through scan-matching algorithms. The backend maintains a chronological pose graph, linking all submaps and trajectory nodes. It continuously searches for topological closures, ultimately solving a large-scale nonlinear optimization problem to minimize global drift and produce a metrically consistent environment representation.

Project Structure & Module Responsibilities

The codebase is organized into functional domains to separate algorithmic logic from system integration:

• mapping: Core algorithmic implementations including 2D/3D trajectory builders, constraint managers, pose graph solvers, and scan matching engines.
• sensor: Abstraction layers for raw data handling, covering point clouds, IMU readings, odometry streams, and compression utilities.
• transform: Rigid body mathematics, coordinate frame conversions, and temporal synchronization utilities.
• common: Foundational libraries featuring thread pools, task queues, configuration parsers, and mathematical helpers.
• configuration_files: Lua-based parameter definitions that control algorithm thresholds, grid resolutions, and optimization weights.

Pipeline Initialization

System startup revolves around instantiating the core mapping engine, configuring sensor pipelines, and launching trajectory execution. The initialization routine handles buffer allocation for coordinate frame broadcasting, loads configuration parameters into structured objects, binds the mapping backend to the frontend, and establishes ROS communication channels.

void InitializeSlamPipeline() {
  // Configure transform buffer with a fixed cache window
  constexpr double kTransformBufferWindowSec = 10.0;
  tf2_ros::Buffer coordinate_buffer(::ros::Duration(kTransformBufferWindowSec));
  tf2_ros::TransformListener frame_monitor(coordinate_buffer);

  FrameworkConfig system_cfg;
  TrajectoryParams trajectory_cfg;
  std::tie(system_cfg, trajectory_cfg) = LoadConfiguration(
      FLAGS_config_path, FLAGS_config_filename);

  // Instantiate the core mapping engine
  auto mapping_core = mapping::CreateMapBuilder(system_cfg.mapping_opts);

  // Transfer ownership of the mapping engine to the main node
  Node runtime_node(system_cfg, std::move(mapping_core), 
                    &coordinate_buffer, FLAGS_enable_metrics);

  // Optionally restore previous session state
  if (!FLAGS_restore_filename.empty()) {
    runtime_node.ImportSessionState(FLAGS_restore_filename, 
                                    FLAGS_apply_frozen_constraints);
  }

  // Launch default sensor topic subscriptions if enabled
  if (FLAGS_use_default_sensor_topics) {
    runtime_node.ActivateDefaultTrajectory(trajectory_cfg);
  }

  ::ros::spin();

  // Gracefully terminate active streams
  runtime_node.TerminateActiveTrajectories();
  
  // Execute final bundle adjustment
  runtime_node.FinalizeOptimization();

  // Persist current session to disk
  if (!FLAGS_export_filename.empty()) {
    runtime_node.ExportSessionState(FLAGS_export_filename, 
                                    true /* retain_pending_submaps */);
  }
}

Sensor Data Ingestion & Coordinate Alignment

Incoming ROS messages undergo temporal alignment and rigid transformation to align all measurements to a unified tracking frame. This ensures that odometry, inertial measurements, GPS fixes, and laser returns share a consistent spatial reference before entering the state estimation pipeline. The sensor bridge routes each modality to its respective handler, validates temporal ordering, and converts message types into framework-native structures.

void InertialNavigationBridge::ProcessOdometryStream(
    const std::string& device_label, 
    const nav_msgs::Odometry::ConstPtr& measurement) {
  
  auto parsed_data = ConvertToFrameworkOdometry(measurement);
  if (!parsed_data) return;

  // Validate temporal sequence to prevent out-of-order processing
  if (IsStaleTimestamp(device_label, parsed_data->capture_time)) {
    LOG(WARNING) << "Dropping stale odometry frame from " 
                 << device_label;
    return;
  }

  UpdateLastSeenTimestamp(device_label, parsed_data->capture_time);

  // Push aligned pose into the trajectory builder pipeline
  active_trajectory_->FeedSensorMeasurement(
      device_label,
      FrameworkOdometry{parsed_data->capture_time, parsed_data->orientation}
  );
}

Frontend Preprocessing Pipeline

Raw lidar returns must undergo several transformations before they can contribute to map updates. The pipeline first synchronizes asynchronous point clouds into a unified temporal batch. It then applies motion compensation by projecting each individual point to a reference timestamp using the pose extrapolator. Points falling outside configured minimum/maximum range thresholds are either discarded or projected as free-space rays. Finally, spatial downsampling via voxel filtering reduces computational overhead without sacrificing environmental detail.

ScanResult LocalMapper2D::IngestPointBatch(
    const std::string& stream_id,
    const sensor::TimedPointCloudData& raw_batch) {
  
  auto aligned_batch = temporal_sync_.SynchronizeStream(stream_id, raw_batch);
  if (aligned_batch.measurements.empty()) {
    return nullptr;
  }

  if (!state_predictor_) {
    InitializeExtrapolator(aligned_batch.capture_time);
  }

  std::vector<:rigid3f> point_poses;
  for (const auto& pt : aligned_batch.measurements) {
    common::Time stamp = aligned_batch.capture_time + 
                         common::FromSeconds(pt.time_offset);
    point_poses.push_back(
        state_predictor_->PredictPose(stamp).cast<float>()
    );
  }

  // Accumulate and filter returns
  ++accumulation_count_;
  for (size_t i = 0; i < aligned_batch.measurements.size(); ++i) {
    transform::Rigid3f point_transform = point_poses[i];
    sensor::LidarHit local_hit = point_transform * 
                                 sensor::ToHit(aligned_batch.measurements[i]);
    sensor::LidarOrigin local_origin = point_transform * 
                                       aligned_batch.origins[...];

    float distance = (local_hit.position - local_origin.position).norm();
    if (distance >= config_.min_range() && distance <= config_.max_range()) {
      accumulated_scan_.valid_hits.push_back(local_hit);
    } else if (distance > config_.max_range()) {
      // Project max-range miss for free-space clearing
      local_hit.position = local_origin.position + 
                           (distance_scale * (local_hit.position - local_origin.position));
      accumulated_scan_.invalid_hits.push_back(local_hit);
    }
  }

  if (accumulation_count_ >= config_.frames_per_scan()) {
    transform::Rigid3d gravity_frame = transform::Rigid3d::Rotation(
        state_predictor_->EstimateGravityAlignment(aligned_batch.capture_time)
    );

    accumulation_count_ = 0;
    accumulated_scan_.origin = point_poses.back().translation();

    return ProcessOptimizedScan(
        aligned_batch.capture_time,
        ApplyGravityFilter(accumulated_scan_, gravity_frame),
        gravity_frame
    );
  }

  return nullptr;
}</:rigid3f>

Pose Extrapolation & State Prediction

The extrapolation module maintains a continuous state estimate between discrete scan matches. It fuses IMU angular velocities for rapid orientation updates and derives translational velocity from either wheel odometry or historical pose deltas. The module caches predicted poses and continuously integrates rotational acceleration, falling back to kinematic differantiation when inertial data is unavailable.

Submap Construction & Grid Updates

Localized environment representations are maintained as independent submaps. In 2D mode, the framework utilizes probability grids where each cell stores log-odds occupancy values. When a scan matches a submap, the grid expands dynamically if new returns exceed current boundaries. Rays originating from the scanner position are traced across the grid: endpoints increment hit probabilities, while traversed cells increment miss probabilities. This probabilistic formulation naturally handles sensor noise and overlapping scans.

void ProbabilityGrid2D::TraceRays(const sensor::RangeScan& scan_data,
                                  const OccupancyLookup& hit_weights,
                                  const OccupancyLookup& miss_weights) {
  
  // Expand grid boundaries if scan exceeds current limits
  AdjustGridDimensions(scan_data);

  const GridBoundaries bounds = GetLimits();
  
  for (const auto& valid_point : scan_data.returns) {
    auto cell_index = bounds.WorldToCellIndex(valid_point.position);
    ApplyProbabilityUpdate(cell_index, hit_weights.LookupProbability());
  }

  for (const auto& free_ray : scan_data.misses) {
    auto ray_cells = TraceLineToTarget(GetScannerOrigin(), free_ray.position);
    for (const auto& cell : ray_cells) {
      ApplyProbabilityUpdate(cell, miss_weights.LookupProbability());
    }
  }
  
  CommitPendingUpdates();
}

Scan Matching Engines

The frontend employs a two-stage matching strategy. The initial stage uses a real-time correlative search that discretizes the pose space. It pre-rotates the point cloud across an angular window, translates it across a spatial grid, and scores candidates by projecting points onto the probability grid. This brute-force approach guarantees a local minimum but provides the initial guess for the second stage.

The refinement stage formulates scan matching as a nonlinear least-squares problem solved via Ceres. Residuals are constructed from occupied space costs, translation constraints, and rotation priors. The optimizer iteratively adjusts the pose to minimize alignment error against the submap's probability distribution, yielding sub-cell precision.

Backend Optimization & Loop Closure

Once submaps accumulate sufficient data, they are marked as inactive and inserted into the global pose graph. The backend continuously evaluates topological constraints by comparing new scan nodes against historical submaps. A fast correlator employs a multi-resolution grid stack and branch-and-bound pruning to efficiently locate potential loop closures without exhaustive searching. Validated constraints are added as edges in the graph, and Ceres-based bundle adjustment optimizes all node poses simultaneously, distributing accumulated error across the trajectory.

OptimalCandidate PoseGraph2D::EvaluateBranchAndBound(
    const std::vector<GridProjection>& projected_scans,
    const SearchConfig& config,
    const std::vector<CandidateNode>& search_queue,
    const int current_depth,
    const float current_threshold) const {

  // Base case: deepest resolution reached
  if (current_depth == 0) {
    return search_queue.front();
  }

  CandidateNode best_leaf(config.spatial_offset(), config.angular_offset(), 
                          0, config);
  best_leaf.confidence_score = current_threshold;

  for (const auto& node : search_queue) {
    if (node.confidence_score <= best_leaf.confidence_score) {
      break; // Prune branches below threshold
    }

    std::vector<CandidateNode> refined_children;
    int stride = 1 << (current_depth - 1);

    // Generate child nodes at half-resolution offsets
    for (int dx : {0, stride}) {
      if (node.x_offset + dx > config.max_bounds.x) break;
      for (int dy : {0, stride}) {
        if (node.y_offset + dy > config.max_bounds.y) break;
        refined_children.emplace_back(node.scan_idx, node.x_offset + dx, 
                                      node.y_offset + dy, config);
      }
    }

    ScoreProjections(resolution_cache_.At(current_depth - 1),
                     projected_scans, config, &refined_children);

    // Recursively evaluate highest-scoring child
    best_leaf = std::max(best_leaf, 
                         EvaluateBranchAndBound(projected_scans, config, 
                                                refined_children, 
                                                current_depth - 1, 
                                                best_leaf.confidence_score));
  }

  return best_leaf;
}

Trajectory Finalization & State Serialization

When a mapping session concludes, the system terminates active sensor streams, flushes remaining optimization tasks, and serializes the complete state. The export routine traverses all trajectories, extracting node poses, submap constraints, IMU/odometry histories, and grid data into a compressed protobuf stream. This serialized state enables offline analysis, map merging, and incremental re-localization without recomputing historical optimization steps.