# cartographer代码解读

**Repository Path**: lijian8/cartographer_code

## Basic Information

- **Project Name**: cartographer代码解读
- **Description**: No description available
- **Primary Language**: C++
- **License**: Apache-2.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 2
- **Created**: 2020-09-04
- **Last Updated**: 2020-12-19

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# cartographer代码解读
## 0. 说明
* 只列主要示意性代码，其他代码省略
* 以处理LaserScan, 2D为例
* 采用Collator（参数map_builder.collate_by_trajectory 为false）
* 使用 --> 表示后续函数调用，或经过某个变量调用的后续函数
* 使用 <-- 表示在基类定义下，实际使用时的子类
* 使用 >>> 表示topic的回调函数中的后续函数调用

<a name="overview"></a>
## 1. 总览
* 主函数 [run](#run)
    + Node node(..., new MapBuilder(...), ...)
    + node --> [Node](#Node)::[StartTrajectoryWithDefaultTopics](#StartTrajectoryWithDefaultTopics)
    + ros::spin
    + node --> [Node](#Node)::[FinishAllTrajectories](#FinishAllTrajectories)
    + node --> [Node](#Node)::[RunFinalOptimization](#RunFinalOptimization)

* [Node](#Node) 类
    + 成员变量
        * [MapBuilderBridge](#MapBuilderBridge) map_builder_bridge_;
    + 构造函数 Node(..., map_builder, ...)
        * 设置成员变量 map_builder_bridge_(..., map_builder, ...)
    + 成员函数 [StartTrajectoryWithDefaultTopics](#StartTrajectoryWithDefaultTopics) 及后续调用
        * 初始化 [MapBuilderBridge](#MapBuilderBridge) 内容
            + map_builder_bridge_ --> 调用 [MapBuilderBridge](#MapBuilderBridge)::[AddTrajectory](#AddTrajectory)
        * 设置消息回调函数
            + scanTopic >>> [HandleLaserScanMessage](#HandleLaserScanMessage)
            + imuTopic  >>> [HandleImuMessage](#HandleImuMessage)
            + odomTopic >>> [HandleOdometryMessage](#HandleOdometryMessage)
    + 成员函数 [HandleLaserScanMessage](#HandleLaserScanMessage)/[HandleImuMessage](#HandleImuMessage)/[HandleOdometryMessage](#HandleOdometryMessage)
        * map_builder_bridge_.sensor_bridges_[trajectory_id] --> 分别调用[SensorBridge](#SensorBridge)::[HandleLaserScanMessage](#s_HandleLaserScanMessage)/[HandleImuMessage](#s_HandleImuMessage)/[HandleOdometryMessage](#s_HandleOdometryMessage)
    + 成员函数 [FinishAllTrajectories](#FinishAllTrajectories)
        * map_builder_bridge_ --> 调用 [MapBuilderBridge](#MapBuilderBridge)::[FinishTrajectory](#FinishTrajectory)
    + 成员函数 [RunFinalOptimization](#RunFinalOptimization)
        * map_builder_bridge_ --> 调用 [MapBuilderBridge](#MapBuilderBridge)::[RunFinalOptimization](#b_RunFinalOptimization)

* [MapBuilderBridge](#MapBuilderBridge) 类
    + 成员变量
        * [MapBuilder](#MapBuilder)* map_builder_;
        * map<trajectory_id, [SensorBridge](#SensorBridge)*> sensor_bridges_;
    * 构造函数 MapBuilderBridge(..., map_builder, ...)
        * 设置成员变量 map_builder_ = map_builder;
    + 成员函数 [AddTrajectory](#AddTrajectory)
        * 初始化 [MapBuilder](#MapBuilder) 内容
            + map_builder_ --> 调用 [MapBuilder](#MapBuilder)::[AddTrajectoryBuilder](#AddTrajectoryBuilder)，获得trajectory_id
        * 添加 SensorBridge(...,  [MapBuilder](#MapBuilder)::[GetTrajectoryBuilder](#GetTrajectoryBuilder)(trajectory_id) <-- CollatedTrajectoryBuilder);
    + 成员函数 [FinishTrajectory](#FinishTrajectory)
        * map_builder_ --> 调用 [MapBuilder](#MapBuilder)::[FinishTrajectory](#b_FinishTrajectory)
    + 成员函数 [RunFinalOptimization](#b_RunFinalOptimization)
        * map_builder_->pose_graph() --> 调用  [PoseGraph2D](#PoseGraph2D)::[RunFinalOptimization](#p_RunFinalOptimization)

* [SensorBridge](#SensorBridge) 类
    + 成员变量
        * TrajectoryBuilderInterface* (<-- [CollatedTrajectoryBuilder](#CollatedTrajectoryBuilder)) trajectory_builder_;
    + 构造函数 SensorBridge(..., trajectory_builder)
        * 设置成员变量 trajectory_builder_ = trajectory_builder;
    + 成员函数 [HandleLaserScanMessage](#s_HandleLaserScanMessage)/[HandleImuMessage](#s_HandleImuMessage)/[HandleOdometryMessage](#s_HandleOdometryMessage)
        * trajectory_builder_ --> 调用  [CollatedTrajectoryBuilder](#CollatedTrajectoryBuilder)::[AddSensorData](#AddSensorData)

* [MapBuilder](#MapBuilder) 类
    + 成员变量
        * vector <TrajectoryBuilderInterface* <-- [CollatedTrajectoryBuilder](#CollatedTrajectoryBuilder)> trajectory_builders_;
        * PoseGraph* (<-- [PoseGraph2D](#PoseGraph2D)) pose_graph_;
        * CollatorInterface* (<-- [Collator](#Collator)) sensor_collator_;
        * ThreadPool thread_pool_;
    + 构造函数 MapBuilder(...)
        * thread_pool_(options.num_background_threads);
        * 根据配置设置成员变量pose_graph_ 和 sensor_collator_，以开始所假设的配置
        * pose_graph_ = new PoseGraph2D(..., new optimization::OptimizationProblem2D(...), ...);
        * sensor_collator_ = new Collator();
    + 成员函数 [AddTrajectoryBuilder](#AddTrajectoryBuilder)
        * int trajectory_id = trajectory_builders_.size();
        * local_trajectory_builder = new [LocalTrajectoryBuilder2D](#LocalTrajectoryBuilder2D)(...);
        * g_trajectory_builder = new [GlobalTrajectoryBuilder](#GlobalTrajectoryBuilder)(local_trajectory_builder, trajectory_id, pose_graph_, res_callback);
        * trajectory_builders_.push_back( new [CollatedTrajectoryBuilder](#CollatedTrajectoryBuilder)(..., sensor_collator_, trajectory_id, ..., g_trajectory_builder) );
    + 成员函数 [GetTrajectoryBuilder](#GetTrajectoryBuilder)(trajectory_id)
        * return trajectory_builders_[trajectory_id];
    + 成员函数 [FinishTrajectory](#b_FinishTrajectory)(trajectory_id)
        * sensor_collator_ --> 调用[Collator](#Collator)::[FinishTrajectory](#c_FinishTrajectory)(trajectory_id);
        * pose_graph_ --> 调用 [PoseGraph2D](#PoseGraph2D)::[FinishTrajectory](#p_FinishTrajectory)(trajectory_id);

* [CollatedTrajectoryBuilder](#CollatedTrajectoryBuilder) 类
    + 成员变量
        * int trajectory_id_;
        * CollatorInterface* (<-- [Collator](#Collator)) sensor_collator_;
        * TrajectoryBuilderInterface* (<-- [GlobalTrajectoryBuilder](#GlobalTrajectoryBuilder)) wrapped_trajectory_builder_;
    + 构造函数 CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, ..., wrapped_trajectory_builder)
        * trajectory_id_ = trajectory_id;
        * sensor_collator_ = sensor_collator;
        * wrapped_trajectory_builder_ = wrapped_trajectory_builder;
        * sensor_collator_ --> 调用[Collator](#Collator)::[AddTrajectory](#c_AddTrajectory)(trajectory_id, ..., HandleCollatedSensorData);
    + 成员函数 [AddSensorData](#AddSensorData)(sensor_id, raw_data)
        * sensor_collator_ --> 调用[Collator](#Collator)::[AddSensorData](#c_AddSensorData)(MakeDispatchable(sensor_id, raw_data))
    + 成员函数 [HandleCollatedSensorData](#HandleCollatedSensorData)(data)
        * data --> 调用[Dispatchable](#Dispatchable)::[AddToTrajectoryBuilder](#d_AddToTrajectoryBuilder)(wrapped_trajectory_builder_);

* [Dispatchable](#Dispatchable) 类
    + 成员变量
        * string sensor_id_ (基类Data的成员);
        * DataType data_;
    
    + 构造函数 Dispatchable(string sensor_id, DataType data)
        * 设置相应成员变量;

    + 成员函数 [AddToTrajectoryBuilder](#d_AddToTrajectoryBuilder)(TrajectoryBuilderInterface* trajectory_builder)
        * trajectory_builder --> 调用 [GlobalTrajectoryBuilder](#GlobalTrajectoryBuilder)::[AddSensorData](#g_AddSensorData1)(sensor_id_, data_);

* [Collator](#Collator) 类
    + 成员变量
        * OrderedMultiQueue queue_;
    + 成员函数 [AddTrajectory](#c_AddTrajectory)(trajectory_id, topicIDList, callback)
        * key = QueueKey{trajectory_id, topicID};
        * queue_.AddQueue(key, callback);
    + 成员函数 [AddSensorData](#c_AddSensorData)(topicID, data)
        * key = QueueKey{trajectory_id, topicID};
        * queue_.Add(key, data) --> OrderedMultiQueue::Dispatch --> [CollatedTrajectoryBuilder](#CollatedTrajectoryBuilder)::[HandleCollatedSensorData](#HandleCollatedSensorData)
    + 成员函数 [FinishTrajectory](#c_FinishTrajectory)(trajectory_id)
        * queue_.MarkQueueAsFinished(queue_key);

* [GlobalTrajectoryBuilder](#GlobalTrajectoryBuilder) 类
    + 成员变量
        * int trajectory_id_;
        * PoseGraph* pose_graph_;
        * LocalTrajectoryBuilder* local_trajectory_builder_;
        * LocalSlamResultCallback local_slam_result_callback_;
    + 构造函数 GlobalTrajectoryBuilder(local_trajectory_builder, trajectory_id, pose_graph_, result_callback)
        * 设置相应成员变量;

    + 成员函数 [AddSensorData](#g_AddSensorData1)(sensor_id, timed_point_cloud_data)
        * local_trajectory_builder_ --> 调用 matching_result = [LocalTrajectoryBuilder2D](#LocalTrajectoryBuilder2D)::[AddRangeData](#L_AddRangeData)(sensor_id, timed_point_cloud_data);
        * pose_graph_ -->  调用 node_id = [PoseGraph2D](#PoseGraph2D)::[AddNode](#AddNode)(matching_result..., trajectory_id_, ...);
        * local_slam_result_callback_(...);
    + 成员函数 [AddSensorData](#g_AddSensorData2)(sensor_id, imu_data)
        * local_trajectory_builder_ --> 调用 [LocalTrajectoryBuilder2D](#LocalTrajectoryBuilder2D)::[AddImuData](#L_AddImuData)(imu_data);
        * pose_graph_ -->  调用 [PoseGraph2D](#PoseGraph2D)::[AddImuData](#p_AddImuData)(trajectory_id_, imu_data);
    + 成员函数 [AddSensorData](#g_AddSensorData3)(sensor_id, odometry_data)
        * local_trajectory_builder_ --> 调用 [LocalTrajectoryBuilder2D](#LocalTrajectoryBuilder2D)::[AddOdometryData](#L_AddOdometryData)(odometry_data);
        * pose_graph_ -->  调用 [PoseGraph2D](#PoseGraph2D)::[AddOdometryData](#p_AddOdometryData)(trajectory_id_, odometry_data);

* [LocalTrajectoryBuilder2D](#LocalTrajectoryBuilder2D) 类
    + 成员变量
        * PoseExtrapolator* extrapolator_;
        * RangeDataCollator range_data_collator_;
        * proto::LocalTrajectoryBuilderOptions2D options_; // TRAJECTORY_BUILDER_2D
        * ActiveSubmaps2D active_submaps_;

    + 成员函数 [AddImuData](#L_AddImuData)(imu_data)
        * [LocalTrajectoryBuilder2D](#LocalTrajectoryBuilder2D)::InitializeExtrapolator(imu_data.time);
        * extrapolator_->AddImuData(imu_data);

    + 成员函数 [AddOdometryData](#L_AddOdometryData)(odometry_data)
        * extrapolator_->AddOdometryData(odometry_data);

    + 成员函数 [AddRangeData](#L_AddRangeData)(sensor_id, unsynchronized_data)
        * 利用range_data_collator_ 调用 [RangeDataCollator](#RangeDataCollator)::[AddRangeData](#r_AddRangeData)将不同传感器的unsynchronized_data按时间戳先后顺序重新编排，获得synchronized_data;
        * 利用extrapolator_进行运动补偿，将不同时刻的synchronized_data变换到统一时刻下;
        * 按距离对数据切割和初始化;
            + 丢弃掉距离小于options_.min_range的点云数据;
            + 将距离小于 options_.max_range数据累积到accumulated_range_data_.returns里;
            + 将距离大于 options_.max_range数据累积到accumulated_range_data_.misses里，
            累积前先把数据的距离变到options_.missing_data_ray_length;
        * 如果累积消息数达到options_.num_accumulated_range_data，则开始处理数据;
            + 调用 TransformToGravityAlignedFrameAndFilter 预处理数据;
            + 调用 AddAccumulatedRangeData 开始正式SLAM处理;
    + 成员函数 [TransformToGravityAlignedFrameAndFilter](#TransformToGravityAlignedFrameAndFilter)
        * 按照重力方向对点云数据进行变换，使得z轴与重力方向重合;
        * 保留z坐标在options_.min_z和options_.max_z间的数据;
        * 用VoxelFilter对点云数据进行滤波，即按照options_.voxel_filter_size参数划分格子，每个格子只保留一个点
    + 成员函数 [AddAccumulatedRangeData](#AddAccumulatedRangeData)
        * 利用 extrapolator_生成预测位姿;
        * 按照 options_.adaptive_voxel_filter里面的参数，用AdaptiveVoxelFilter对点云数据进行滤波;
        * 用预测位姿，将滤波后的点云数据和地图做 ScanMatch(预测位姿, 点云数据)，获得位姿pose_estimate;
        * extrapolator_->AddPose(time, pose_estimate);
        * 利用pose_estimate对点云数据变换到地图坐标系下，InsertIntoSubmap(地图坐标系下的点云数据, ...);

    + 成员函数 [ScanMatch](#ScanMatch)(预测位姿，点云数据)
        * 取当前活跃子地图列表中的最新一个作为 matching_submap;
        * initial_ceres_pose = pose_prediction;
        * 如果 options_.use_online_correlative_scan_matching 为真，按照 options_.real_time_correlative_scan_matcher 里面的参数，调用 [RealTimeCorrelativeScanMatcher2D](#RealTimeCorrelativeScanMatcher2D) ::[Match](#r_Match)(预测位姿, 点云数据, matching_submap)，修改initial_ceres_pose;
        * 按照 options_.ceres_scan_matcher 里面的参数，调用 [CeresScanMatcher2D](#CeresScanMatcher2D)::[Match](#c_Match)(预测位姿, initial_ceres_pose, 点云数据, matching_submap)，获得最终位姿;

    + 成员函数 [InsertIntoSubmap](#InsertIntoSubmap)(地图坐标系下的点云数据)
        * active_submaps_ --> 调用 [ActiveSubmaps2D](#ActiveSubmaps2D)::InsertRangeData(地图坐标系下的点云数据);

* [ActiveSubmaps2D](#ActiveSubmaps2D) 类
    + 成员变量
    + 成员函数

* [PoseGraph2D](#PoseGraph2D) 类
    + 成员函数 [AddNode](#AddNode)
    + 成员函数 [AddImuData](#p_AddImuData)
    + 成员函数 [AddOdometryData](#p_AddOdometryData)
    + 成员函数 [FinishTrajectory](#p_FinishTrajectory)(trajectory_id)

* [AdaptiveVoxelFilter](#AdaptiveVoxelFilter) 类
    + 成员函数 Filter及后续调用函数FilterByMaxRange和AdaptivelyVoxelFiltered
        + 剔除距离大于 adaptive_voxel_filter.max_range的点;
        + 如果点云数小于 adaptive_voxel_filter.min_num_points，返回当前点云;
        + 用 adaptive_voxel_filter.max_length 为参数，用VoxelFilter对点云数据进行滤波，如果点云数大于 adaptive_voxel_filter.min_num_points（点数足够密），则返回当前点云;
        + 从 adaptive_voxel_filter.max_length 开始，做二分查找VoxelFilter参数，使得滤波后的点云数点云数尽量接近 adaptive_voxel_filter.min_num_points;

* [RealTimeCorrelativeScanMatcher2D](#RealTimeCorrelativeScanMatcher2D) 类
    + 成员函数 [Match](#r_Match)

* [CeresScanMatcher2D](#CeresScanMatcher2D) 类
    + 成员函数 [Match](#c_Match)

<a name="cartographer_ros"></a>
## 2. cartographer_ros


<a name="run"></a>
### 2.1 主函数 Run (node_main.cc) [返回总览](#overview)

```
Node node(..., new MapBuilder(...), ...)
node.StartTrajectoryWithDefaultTopics(...)

::ros::spin()

node.FinishAllTrajectories();
node.RunFinalOptimization();
```

<a name="Node"></a>
### 2.2 主节点类 Node (node.h, node.cc)

* 成员变量
```
MapBuilderBridge* map_builder_bridge_;
```

* Node(..., map_builder, ...)

```
设置成员变量 map_builder_bridge_(..., map_builder, ...)
```

<a name="StartTrajectoryWithDefaultTopics"></a>
* StartTrajectoryWithDefaultTopics(...) --> AddTrajectory(...)  [返回总览](#overview)
```
map_builder_bridge_.AddTrajectory(topicIDList, ...);
AddExtrapolator(...);
LaunchSubscribers(...);
```

<a name="LaunchSubscribers"></a>
* LaunchSubscribers(...)  [返回总览](#overview)
```
scanTopic >>> HandleLaserScanMessage

if (trajectory_builder_2d.use_imu_data)
    imuTopic >>> HandleImuMessage

if (options.use_odometry)
    odomTopic >>> HandleOdometryMessage
```

<a name="HandleLaserScanMessage"></a>
* HandleLaserScanMessage(trajectory_id, ...) [返回总览](#overview)
```
map_builder_bridge_.sensor_bridge_[trajectory_id]->HandleLaserScanMessage(...);
```

<a name="HandleImuMessage"></a>
* HandleImuMessage(trajectory_id, ...) [返回总览](#overview)
```
extrapolators.AddImuData(...);
map_builder_bridge_.sensor_bridge_[trajectory_id]->HandleImuMessage(...);
```

<a name="HandleOdometryMessage"></a>
* HandleOdometryMessage(trajectory_id, ...) [返回总览](#overview)
```
extrapolators.AddOdometryData(...);
map_builder_bridge_.sensor_bridge_[trajectory_id]->HandleOdometryMessage(...);
```

<a name="FinishAllTrajectories"></a>
* FinishAllTrajectories() [返回总览](#overview)
```
...
map_builder_bridge_.FinishTrajectory(...);
```

<a name="RunFinalOptimization"></a>
* RunFinalOptimization() [返回总览](#overview)
```
...-->map_builder_bridge_.FinishTrajectory(...);

map_builder_bridge_.RunFinalOptimization();
```

<a name="MapBuilderBridge"></a>
### 2.3 MapBuilderBridge 类 (map_builder_bridge.h, map_builder_bridge.cc)

* 成员变量
```
MapBuilder* map_builder_;
map<trajectory_id, SensorBridge*> sensor_bridges_;
```

* MapBuilderBridge(..., map_builder, ...)
```
设置成员变量 map_builder_ = map_builder;
```

<a name="AddTrajectory"></a>
* AddTrajectory(topicIDList, ...) [返回总览](#overview)
```
trajectory_id = map_builder_->AddTrajectoryBuilder(topicIDList, ...);
sensor_bridges_[trajectory_id] = new SensorBridge(..., map_builder_->GetTrajectoryBuilder(trajectory_id));
```

<a name="FinishTrajectory"></a>
* FinishTrajectory(trajectory_id) [返回总览](#overview)
```
map_builder_->FinishTrajectory(trajectory_id);
```

<a name="b_RunFinalOptimization"></a>
* RunFinalOptimization() [返回总览](#overview)
```
map_builder_->pose_graph()->RunFinalOptimization();
```

<a name="SensorBridge"></a>
### 2.4 SensorBridge 类 (sensor_bridge.h, sensor_bridge.cc)

* 成员变量
```
TrajectoryBuilderInterface* (<-- CollatedTrajectoryBuilder) trajectory_builder_
```

* SensorBridge(..., trajectory_builder)
```
设置成员变量 trajectory_builder_ = trajectory_builder;
```

<a name="s_HandleLaserScanMessage"></a>
* HandleLaserScanMessage(...) --> HandleLaserScan(...) [返回总览](#overview)
```
for (num_subdivisions_per_laser_scan){
    set subdivision data;
    HandleRangefinder(topicID, ..., subdivision) --> trajectory_builder_->AddSensorData(topicID, TimedPointCloudData);
}
```

<a name="s_HandleImuMessage"></a>
* HandleImuMessage(...) [返回总览](#overview)
```
trajectory_builder_->AddSensorData(topicID, ImuData);
```

<a name="s_HandleOdometryMessage"></a>
* HandleOdometryMessage(...) [返回总览](#overview)
```
trajectory_builder_->AddSensorData(topicID, OdometryData);
```

<a name="cartographer"></a>
## 3. cartographer

<a name="MapBuilder"></a>
### 3.1 MapBuilder 类 (map_builder.h, map_builder.cc)

* 成员变量
```
vector <TrajectoryBuilderInterface* <--CollatedTrajectoryBuilder> trajectory_builders_
PoseGraph* (<-- PoseGraph2D) pose_graph_
CollatorInterface* (<-- Collator) sensor_collator_
```

* MapBuilder(...)
```
if (map_builder.use_trajectory_builder_2d){
    pose_graph_ = new PoseGraph2D(..., new optimization::OptimizationProblem2D(...), ...);
}
if (map_builder.use_trajectory_builder_3d){
    pose_graph_ = new PoseGraph3D(..., new optimization::OptimizationProblem3D(...), ...);
}

if (map_builder.collate_by_trajectory)
    sensor_collator_ = new TrajectoryCollator();
else
    sensor_collator_ = new Collator();
```


<a name="AddTrajectoryBuilder"></a>
* AddTrajectoryBuilder(topicIDList, ...) [返回总览](#overview)
```
int trajectory_id = trajectory_builders_.size();
if (map_builder.use_trajectory_builder_3d){
    if (!map_builder.has_trajectory_builder_3d_options)
        local_trajectory_builder = new LocalTrajectoryBuilder3D(topicIDList, ...)

    trajectory_builders_.push_back( CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, topicIDList, 
            new GlobalTrajectoryBuilder(local_trajectory_builder, trajectory_id, pose_graph, result_callback)  )  );
}else{
    if (!map_builder.has_trajectory_builder_2d_options)
        local_trajectory_builder = new LocalTrajectoryBuilder2D(topicIDList, ...)

    trajectory_builders_.push_back( CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, topicIDList, 
            new GlobalTrajectoryBuilder(local_trajectory_builder, trajectory_id, pose_graph, result_callback)  )  );
}

MaybeAddPureLocalizationTrimmer(trajectory_id, ..., pose_graph_);

if (trajectory_builder.has_initial_trajectory_pose)
    pose_graph_->SetInitialTrajectoryPose(trajectory_id, trajectory_builder.initial_trajectory_pose, ...);
```

<a name="GetTrajectoryBuilder"></a>
* GetTrajectoryBuilder(trajectory_id) [返回总览](#overview)
```
return trajectory_builders_[trajectory_id];  // CollatedTrajectoryBuilder
```

<a name="b_FinishTrajectory"></a>
* FinishTrajectory(...) [返回总览](#overview)
```
sensor_collator_->FinishTrajectory(trajectory_id);
pose_graph_->FinishTrajectory(trajectory_id);
```

<a name="CollatedTrajectoryBuilder"></a>
### 3.2 CollatedTrajectoryBuilder 类 (collated_trajectory_builder.h, collated_trajectory_builder.cc)

* CollatedTrajectoryBuilder(..., sensor_collator, trajectory_id, topicIDList, wrapped_trajectory_builder)
```
sensor_collator->AddTrajectory(trajectory_id, topicIDList, HandleCollatedSensorData);
```

<a name="HandleCollatedSensorData"></a>
* HandleCollatedSensorData(topicID, data) [返回总览](#overview)
```
data->AddToTrajectoryBuilder(wrapped_trajectory_builder); // GlobalTrajectoryBuilder
```

<a name="AddSensorData"></a> 
* AddSensorData(topicID, TimedPointCloudData/ImuData/OdometryData) --> AddData --> sensor_collator->AddSensorData


<a name="Collator"></a>
### 3.3 Collator 类 (collator.h, collator.cc) // sensor_collator

<a name="c_AddTrajectory"></a>
* AddTrajectory(trajectory_id, topicIDList, callback) [返回总览](#overview)
```
for (topicID: topicIDList){
    queue_key = QueueKey{trajectory_id, topicID};
    queue_.AddQueue(queue_key, callback);
}
```

<a name="c_AddSensorData"></a>
* AddSensorData(topicID, data) [返回总览](#overview)
```
    queue_key = QueueKey{trajectory_id, topicID};
    queue_.Add(queue_key, data) --> Dispatch --> callback
```

<a name="GlobalTrajectoryBuilder"></a>
### 3.4 GlobalTrajectoryBuilder 类 (map_builder.h, map_builder.cc)

<a name="LocalTrajectoryBuilder2D"></a>
### 3.5 LocalTrajectoryBuilder2D 类 (map_builder.h, map_builder.cc)

<a name="PoseGraph2D"></a>
### 3.6 PoseGraph2D 类 (map_builder.h, map_builder.cc)

<a name="p_RunFinalOptimization"></a>
* RunFinalOptimization(...) [返回总览](#overview)
```

```