cartographer中创建轨迹

开始一条轨迹

主要函数，AddTrajectoryBuilder

目的就是创建一个新的TrajectoryBuilder 并返回它的 trajectory_id，cartographer会创建不止一条轨迹，每一条轨迹都会有一个id。

需要的输入参数：

expected_sensor_ids 所有需要的topic的名字的集合

trajectory_options 轨迹的参数配置

local_slam_result_callback 需要传入的回调函数，实际上是map_builder_bridge.cc 中的OnLocalSlamResult() 函数

输出：

return int 新生成的轨迹的id

AddTrajectoryBuilder函数的调用过程

​ 还是从node.cc文件开始

1. node_main.cc的StartTrajectoryWithDefaultTopics函数

   node.StartTrajectoryWithDefaultTopics(trajectory_options);
   1

2. Node类的StartTrajectoryWithDefaultTopics函数

   void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
     absl::MutexLock lock(&mutex_);
     // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
     CHECK(ValidateTrajectoryOptions(options));
     // 添加一条轨迹
     AddTrajectory(options);
   }
   1
   2
   3
   4
   5
   6
   7

3. 上面函数中有一个AddTrajectory（options）函数

   const int trajectory_id =
         map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
   1
   2

   其中又包括map_builder_bridge_.AddTrajectory

   map_builder_bridge_是由MapBuilderBridge这个类创建，这个类里面有一个AddTrajectory成员函数

4. MapBuilderBridge::AddTrajectory

   int MapBuilderBridge::AddTrajectory() {
     // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
     const int trajectory_id = map_builder_->AddTrajectoryBuilder(
         expected_sensor_ids, trajectory_options.trajectory_builder_options,
         // lambda表达式 local_slam_result_callback_
         [this]() {
           // 保存local slam 的结果数据 5个参数实际只用了4个
           OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
         });
     LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
   
     // Make sure there is no trajectory with 'trajectory_id' yet.
     CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
     // Step: 2 为这个新轨迹 添加一个SensorBridge，为sensorbridge初始化
     sensor_bridges_[trajectory_id] = absl::make_unique(); // CollatedTrajectoryBuilder
   1
   2
   3
   4
   5
   6
   7
   8
   9
   10
   11
   12
   13
   14
   15

   至此，AddTrajectoryBuilder在MapBuilderBridge::AddTrajectory()函数中被调用

进入AddTrajectoryBuilder函数

​ 参考带注释的源码

const int trajectory_id = trajectory_builders_.size();
1

id是从零开始的, 所以新trajectory_id就是trajectory_builders_的size()。

absl::optional pose_graph_odometry_motion_filter;
1

在配置文件中没有pose_graph_odometry_motion_filter这个参数，也没有使用后端的里程计motion_filter

构建2d轨迹

else {
    std::unique_ptr local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }

    DCHECK(dynamic_cast(pose_graph_.get()));

    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

在这里，首先建立了一个LocalTrajectoryBuilder2D类的对象local_trajectory_builder（这个类是不带回环的前端slam，但他包括了位姿估计和扫描匹配）然后传入雷达topic和轨迹配置参数对其初始化。

接着通过dynamic_cast将pose_graph_对象强制转换为PoseGraph2D，并检查数据类型是否正确。

接着构建轨迹跟踪器CollatedTrajectoryBuilder2D，传入轨迹id，位姿图等参数，然后将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder，也就是trajectory_builders_.push_back了。

纯定位模式

如果是纯定位模式，调用这个函数，然后仅保存最近的三个submap

 MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                  pose_graph_.get());
1
2

void MaybeAddPureLocalizationTrimmer(
    const int trajectory_id,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    PoseGraph* pose_graph) {
  if (trajectory_options.has_pure_localization_trimmer()) {
    pose_graph->AddTrimmer(absl::make_unique(
        trajectory_id,
        trajectory_options.pure_localization_trimmer().max_submaps_to_keep()));
  }
}
1
2
3
4
5
6
7
8
9
10

在纯定位模式中，在pose_graph_中添加一个PureLocalizationTrimmer类型修饰器，来完成这一功能。

根据has_pure_localization_trimmer参数配置，决定保存多少submap，参数在trajectory_builder。lua文件中，默认注释掉，即默认会生成三个submap。

（之前有一个老参数，已经弃用，会报警告，但不影响使用）