cartographer/sensor/internal/trajectory_collator.h

Overview

TrajectoryCollator is a CollatorInterface implementation that maintains one OrderedMultiQueue per trajectory. It waits to see at least one data item for all expected sensor IDs of a trajectory and then dispatches data in merge-sorted (time) order within that trajectory. Contrary to Collator, it does not wait for other trajectories, and its output ordering across trajectories is not deterministic.

Class: cartographer::sensor::TrajectoryCollator

Construction

TrajectoryCollator() {}

Public Methods

void AddTrajectory(
    int trajectory_id,
    const absl::flat_hash_set<std::string>& expected_sensor_ids,
    const Callback& callback) override;

void FinishTrajectory(int trajectory_id) override;

void AddSensorData(int trajectory_id, std::unique_ptr<Data> data) override;

void Flush() override;

absl::optional<int> GetBlockingTrajectoryId() const override;

static void RegisterMetrics(metrics::FamilyFactory* family_factory);

Implementation Notes

  • AddTrajectory — creates one OrderedMultiQueue for the trajectory and registers a queue per expected sensor ID under a QueueKey{trajectory_id, sensor_id}. Each queue’s callback is bound to the given callback with the sensor ID fixed, so dispatches arrive with the correct sensor ID.

  • FinishTrajectory — marks every queue of the trajectory as finished, so the multi-queue can flush its remaining data.

  • AddSensorData — routes the packet into the trajectory’s queue via OrderedMultiQueue::Add, and increments a per-(sensor, trajectory) metric counter created by GetOrCreateSensorMetric.

  • Flush — flushes every trajectory’s queue.

  • GetBlockingTrajectoryId — always returns nullopt: this collator never blocks on a particular trajectory.

Metrics

static metrics::Family<metrics::Counter>* collator_metrics_family_;
absl::flat_hash_map<std::string, metrics::Counter*> metrics_map_;

RegisterMetrics creates the collator_input_total counter family; GetOrCreateSensorMetric lazily adds one counter per sensor_id/trajectory_id pair and increments it on every received packet.

State

absl::flat_hash_map<int, OrderedMultiQueue> trajectory_to_queue_;
absl::flat_hash_map<int, std::vector<QueueKey>> trajectory_to_queue_keys_;

Role in the SLAM Pipeline

MapBuilder selects TrajectoryCollator when collate_by_trajectory is enabled. Each trajectory’s sensor data is then synchronized independently, so one trajectory does not stall waiting for another trajectory’s data — at the cost of non-deterministic cross-trajectory ordering.

See Also