(02)Cartographer源码无死角解析-(85) 纯定位模式、子图修剪 PureLocalizationTrimmer 与 PoseGraph2D::TrimmingHandle

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

上一篇博客中，分析了如何加载 .pbstream 文件，下面是 node_main.cc 中的代码：

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

  // 如果加载了pbstream文件, 就执行这个函数
  if (!FLAGS_load_state_filename.empty()) {
    
    
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    
    
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

对于 node.LoadState() 分析完成之后，就是对 node.StartTrajectoryWithDefaultTopics(trajectory_options) 的分析了。其实该函数在前面已经讲解过，只是这次讲解的形式不一样，为纯定位模式，首先是输入配置不一样，如本人执行的命令行如下：

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag

roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${
    
    HOME}/Downloads/b2-2016-04-05-14-44-52.bag
roslaunch cartographer_ros demo_backpack_2d_localization.launch \
   load_state_filename:=${
    
    HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
   bag_filename:=${
    
    HOME}/Downloads/b2-2016-04-27-12-31-41.bag

其中 demo_backpack_2d_localization.launch 存在如下内容：

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d_localization.lua
          -load_state_filename $(arg load_state_filename)"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

可以看到，其使用的配置文件是 backpack_2d_localization.lua，其包含纯定位模式的主要配置参数。

二、增加轨迹

大部分内容都在前面分析过了，这里就不再进行重复了，只对与纯定位模型相关的函数进行分析，首先调用：

// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
    
    
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

根据配置参数添加一条轨迹，其上的 Node::AddTrajectory() 函数还是比较重要的，如下所示：

/**
 * @brief 添加一个新的轨迹
 *
 * @param[in] options 轨迹的参数配置
 * @return int 新生成的轨迹的id
 */
int Node::AddTrajectory(const TrajectoryOptions& options) {
    
    

  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);

  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);

  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);

  // 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
  // 检查设置的topic名字是否在ros中存在, 不存在则报错
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    
    
    subscribed_topics_.insert(sensor_id.id);
  }

  return trajectory_id;
}

其首先根据配置参数通过接口 map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); 添加一个轨迹，获得该轨迹的 id，同时为该轨迹绑定一个位姿推断器 AddExtrapolator 以及采样。该处需要注意一点的是，从 .bpstream 中加载的轨迹默认从 0 开始，如果 .bpstream 只存储了一条轨迹，那么这里的 trajectory_id 就是1了。可以通过ROS的可视化直接查看轨迹id。

三、配置文件与参数

关于 src/cartographer_ros/cartographer_ros/configuration_files/backpack_2d_localization.lua 文件内容如下：

include "backpack_2d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
    
    
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

可以看到其上的配置，max_submaps_to_keep 表示其最多存储子图的数量为 3，POSE_GRAPH.optimize_every_n_nodes 表示每增加 20 个节点node 就会进行一次后端优化。其包含的 backpack_2d.lua 又包含了 trajectory_builder.lua，而其又包含了 trajectory_builder_2d.lua、trajectory_builder_3d.lua。这样就把所有需要的配置文件都包含进去了，总的看起来，纯定位模式与建图定位模式似乎没有太大的差异，主要核心差异就是 max_submaps_to_keep = 3。在源码中搜索该参数，可以看到如下代码：

// 检查是否是纯定位模式,支持2种纯定位的参数名字,老参数已经弃用,会报警告但程序不会终止
void MaybeAddPureLocalizationTrimmer(
    const int trajectory_id,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    PoseGraph* pose_graph) {
    
    
  if (trajectory_options.pure_localization()) {
    
    
    LOG(WARNING)
        << "'TrajectoryBuilderOptions::pure_localization' field is deprecated. "
           "Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead.";
    pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
        trajectory_id, 3 /* max_submaps_to_keep */));
    return;
  }
  if (trajectory_options.has_pure_localization_trimmer()) {
    
    
    pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
        trajectory_id,
        trajectory_options.pure_localization_trimmer().max_submaps_to_keep()));
  }
}

这里是添加一个纯定位模式的修剪器，老的配置参数 trajectory_options.pure_localization() 已经被弃用了，我们应该使用新的配置参数 trajectory_options.pure_localization_trimmer().max_submaps_to_keep()。改函数其实还是比较简单的，就是构建一个子图修剪器，然后通过 pose_graph->AddTrimmer() 函数传递给后端。下面来看看 MaybeAddPureLocalizationTrimmer() 是哪里被调用的呢？在新构建一条轨迹的时候，也就是 MapBuilder::AddTrajectoryBuilder() 这个函数，可以看到如下代码：

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

需要注意的是，只有纯定位模式才又配置地图修剪器，建图模式是没有的。

四、PureLocalizationTrimmer

纯定位子图修剪器声名于 src/cartographer/cartographer/mapping/pose_graph_trimmer.h 之中，如下：

class PureLocalizationTrimmer : public PoseGraphTrimmer {
    
    
 public:
  PureLocalizationTrimmer(int trajectory_id, int num_submaps_to_keep);
  ~PureLocalizationTrimmer() override {
    
    }

  void Trim(Trimmable* pose_graph) override;
  bool IsFinished() override;

 private:
  const int trajectory_id_;
  int num_submaps_to_keep_;
  bool finished_ = false;
};

看起来十分简单，前面提到其是为每条需要的轨迹创建子图修剪器，所以需要一个 trajectory_id_ 进行绑定，num_submaps_to_keep_ 不用多说也知道就是配置文件中的参数 max_submaps_to_keep 了。finished_ 表示修剪器的工作是否已经完成了，通常轨迹完成时，其对应的子图修剪器也会被标记为完成，不再进行修剪。其成员函数的实现总体来看还是比较简单的，如下所示：

PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
                                                 const int num_submaps_to_keep)
    : trajectory_id_(trajectory_id), num_submaps_to_keep_(num_submaps_to_keep) {
    
    
  CHECK_GE(num_submaps_to_keep, 2) << "Cannot trim with less than 2 submaps";
}

// 进行子图的裁剪
void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
    
    
  // 如果轨迹处于完成状态, 就将num_submaps_to_keep_设置为0
  if (pose_graph->IsFinished(trajectory_id_)) {
    
    
    num_submaps_to_keep_ = 0;
  }

  // 获取所有的SubmapId
  auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
  // 从序号0开始裁剪submap, 并删除相关的约束与节点
  for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) {
    
    
    pose_graph->TrimSubmap(submap_ids.at(i));
  }

  // 设置轨迹状态为 DELETED
  if (num_submaps_to_keep_ == 0) {
    
    
    finished_ = true;
    pose_graph->SetTrajectoryState(
        trajectory_id_, PoseGraphInterface::TrajectoryState::DELETED);
  }
}

// 获取裁剪器的状态
bool PureLocalizationTrimmer::IsFinished() {
    
     return finished_; }

其核心就是调用了后端的 pose_graph->TrimSubmap() 函数，在讲解之前，先来看一下前面提到的：

// map_builder.cc中调用, 纯定位时添加PureLocalizationTrimmer
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
    
    
  // C++11 does not allow us to move a unique_ptr into a lambda.
  PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
  AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
    
    
    absl::MutexLock locker(&mutex_);
    trimmers_.emplace_back(trimmer_ptr);
    return WorkItem::Result::kDoNotRunOptimization;
  });
}

其就是把 MaybeAddPureLocalizationTrimmer() 函数中创建的子图修建器实参传递给该函数的形参 trimmer。然后赋值给PoseGraph2D::PoseGraphTrimmer这个指针，然后再把该指针添加至 PoseGraph2D::trimmers_ 之中。下面就来看看该私有成员变量再哪里被使用到。

五、TrimmingHandle 构造

再源码中对 trimmers_ 进行搜索，可以看到 PoseGraph2D::HandleWorkQueue() 函数中如下片段：

	......
	RunOptimization();
	......
	TrimmingHandle trimming_handle(this);
    // 进行子图的裁剪, 如果没有裁剪器就不裁剪
    for (auto& trimmer : trimmers_) {
    
    
      trimmer->Trim(&trimming_handle); // PureLocalizationTrimmer::Trim()
    }
    // 如果裁剪器处于完成状态, 就把裁剪器删除掉
    trimmers_.erase(
        // c++11: std::remove_if 如果回调函数函数返回真,则将当前所指向的参数移到尾部,返回值是被移动区域的首个元素
        std::remove_if(trimmers_.begin(), trimmers_.end(),
                       [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
    
    
                         return trimmer->IsFinished(); // 调用PureLocalizationTrimmer::IsFinished()
                       }),
        trimmers_.end());
	......

其首先构建一个 TrimmingHandle 类型的对象。比较有意思的时，其构造函数尽然接受一个 this 指针对象。具体缘由后续再进行分析。接着可以看到其对所有子图修剪器进行了遍历，并且调用子图修剪器的 trimmer->Trim(&trimming_handle) 函数，且传递给该函数的就是 TrimmingHandle 类型实例 trimming_handle，前面知道其有绑定 PoseGraph2D 实例的 this 指针。本质上就是对每条轨迹的子图进行修剪。修剪完制子图之后，其还执行了一个擦除操作，如果子图已经处于完成状态，则直接从 trimmers_ 中擦除该修剪器。

TrimmingHandle 需要注意构造时传递了一个 this 指针，如 TrimmingHandle trimming_handle(this)，这里的 this 表示的就是 PoseGraph2D 类型的实例。关于 TrimmingHandle 的构造函数声名于 src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.h 中，其继承于 Trimmable ，比较简单这里就跳过了，需要注意其存在一个私有成员 PoseGraph2D* const parent_，关于TrimmingHandle 的成员函数都在 pose_graph_2d.cc 中实现。其构造函数如下：

PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent)
    : parent_(parent) {
    
    }

其比较简单的，就是把 parent(this) 指针保存起来。再回过头来看一下前面的代码：

    for (auto& trimmer : trimmers_) {
    
    
      trimmer->Trim(&trimming_handle); // PureLocalizationTrimmer::Trim()
    }

其把构建出来的 trimming_handle 对象传递给了 PoseGraphTrimmer 实例 trimmer 的 Trim 函数，也就是当前轨迹对应的 PureLocalizationTrimmer::Trim()。该代码再前面已经分析，核心就是：

	// 从序号0开始裁剪submap, 并删除相关的约束与节点
	for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) {
    
    
	  pose_graph->TrimSubmap(submap_ids.at(i));
	}

对每个子图都调用了 pose_graph->TrimSubmap() 函数。Trimmable* const pose_graph 就是 TrimmingHandle trimming_handle(this)，内部包含有 PoseGraph2D 实例。如果觉得理解起来，就把 TrimmingHandle 当作时对 PoseGraph2D 部分函数的封装，TrimmingHandle 只能访问 PoseGraph2D 一部分函数，启到隔绝作用。如下

六、TrimmingHandle 成员函数

可以先看一下 TrimmingHandle 的部分成员函数:

	......
	const MapById<NodeId, TrajectoryNode>&
	PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const {
    
    
	  return parent_->data_.trajectory_nodes;
	}
	
	const std::vector<PoseGraphInterface::Constraint>&
	PoseGraph2D::TrimmingHandle::GetConstraints() const {
    
    
	  return parent_->data_.constraints;
	}
	
	// 轨迹结束了, 裁剪器就结束
	bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {
    
    
	  return parent_->IsTrajectoryFinished(trajectory_id);
	}
	......

已知 parent_ 就是 PoseGraph2D 实例，所以其本质上就是调用了 PoseGraph2D 的成员函数或者其 PoseGraph2D 成员变量的成员函数。其大致流程如下：

PoseGraph2D::HandleWorkQueue()
	PureLocalizationTrimmer::Trim(this)  //每条轨迹对应一个该实例 trimmer
		PoseGraph2D::TrimmingHandle::TrimSubmap(submap_ids.at(i)); //每条轨迹的每个子图执行该函数
			PoseGraph2D::.......
			PoseGraph2D::.......
			PoseGraph2D::.......

也就是说，最终都是调通过 TrimmingHandle::TrimSubmap() 函数调用 PoseGraph2D 的函数来实现对子图的剪切。关于 PoseGraph2D::TrimmingHandle::TrimSubmap() 这里先说一下他们流程，然后再给出整体注释：

$\color{red} 核心$ max_submaps_to_keep 为3则表示最多存在三个子图，如果新建了第四个子图，则需要把最前面的子图删除掉。删除一个子图，那么就要删除与该子图相关的约束(子图内、子图间)、以及该子图对应的多分辨率地图、分支定界扫面匹配器等，最后才删除该地图指针与栅格数据等。

$\color{blue}(1)$ 首先 TrimSubmap(submap_ids.at(i)) 是对一个子图进行处理，且只处理已经完成的子图，所以纯定位的 max_submaps_to_keep 参数得大于等于3，因为两个子图为活跃子图，用于插入点云数据。

$\color{blue}(2)$ 获取除 submap_id 外所有子图的所有节点 id，存储于变量 nodes_to_retain 之中，找到在 submap_id 子图内部同时不在别的子图内的节点, 这些节点需要删除，存储于 nodes_to_remove 。就是说这些节点只属于 submap_id。

$\color{blue}(3)$ 接着就是要找到与 nodes_to_remove (属submap_id) 节点相关的约束，也就是对所有约束进行遍历，如果约束对应 node_id 不属于 nodes_to_remove，是则表示不需要删除，记录于 constraints 且赋值给 parent_->data_.constraints(等价于已经删除属于 nodes_to_remove 的约束)。如果遍历的约束属于 nodes_to_remove，则先把这个约束所属的 constraint.submap_id 记录在 other_submap_ids_losing_constraints 之中。注意，此时存储于 parent_->data_.constraints 的约束已经与 submap_id 子图无关。

$\color{blue}(4)$ 考虑 other_submap_ids_losing_constraints 中的子图是否有必要删除，此时对经过一次删除操作的 parent_->data_.constraints 进行遍历，只要 other_submap_ids_losing_constraints 中的子图与 parent_->data_.constraints 存在子图间联系，则该子图都不会删除。反之会删除这些子图的匹配器，但是这个子图是不能删除的，因为其还存在子图内约束。

$\color{blue}(5)$ 删除当前地子图 submap_id 指针、匹配器、多分辨率地图、且从后端优化问题optimization_problem_ 中删除这个子图。最后就是根据 nodes_to_remove 参数移除掉 parent_->data_ 与 parent_->optimization_problem_ 对应的节点。代码整体注释如下：

// 删除指定id的子图, 并删除相关的约束,匹配器,与节点
void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) {
    
    
  // TODO(hrapp): We have to make sure that the trajectory has been finished
  // if we want to delete the last submaps.
  // 只有kFinished状态的子图才能够裁剪
  CHECK(parent_->data_.submap_data.at(submap_id).state ==
        SubmapState::kFinished);

  // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps
  // once the submap with 'submap_id' is gone.
  // We need to use node_ids instead of constraints here to be also compatible
  // with frozen trajectories that don't have intra-constraints.
  // 获取除submap_id外的所有子图的所有节点的id
  std::set<NodeId> nodes_to_retain;
  for (const auto& submap_data : parent_->data_.submap_data) {
    
    
    if (submap_data.id != submap_id) {
    
    
      nodes_to_retain.insert(submap_data.data.node_ids.begin(),
                             submap_data.data.node_ids.end());
    }
  }

  // Remove all nodes that are exlusively associated to 'submap_id'.
  // 找到在submap_id的子图内部同时不在别的子图内的节点, 这些节点需要删除
  std::set<NodeId> nodes_to_remove;
  // c++11: std::set_difference 求set的差集, 在first_set中出现, 在second_set中不出现的元素
  std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(),
                      parent_->data_.submap_data.at(submap_id).node_ids.end(),
                      nodes_to_retain.begin(), nodes_to_retain.end(),
                      std::inserter(nodes_to_remove, nodes_to_remove.begin()));

  // Remove all 'data_.constraints' related to 'submap_id'.
  {
    
    
    // Step: 1 删除submap_id相关的约束
    std::vector<Constraint> constraints;
    for (const Constraint& constraint : parent_->data_.constraints) {
    
    
      if (constraint.submap_id != submap_id) {
    
    
        constraints.push_back(constraint);
      }
    }
    parent_->data_.constraints = std::move(constraints);
  }

  // Remove all 'data_.constraints' related to 'nodes_to_remove'.
  // If the removal lets other submaps lose all their inter-submap constraints,
  // delete their corresponding constraint submap matchers to save memory.
  {
    
    
    std::vector<Constraint> constraints;
    std::set<SubmapId> other_submap_ids_losing_constraints;
    // Step: 2 删除与nodes_to_remove中节点相关联的约束, 并对submap_id进行标记
    for (const Constraint& constraint : parent_->data_.constraints) {
    
    
      if (nodes_to_remove.count(constraint.node_id) == 0) {
    
    
        constraints.push_back(constraint);
      } else {
    
    
        // A constraint to another submap will be removed, mark it as affected.
        other_submap_ids_losing_constraints.insert(constraint.submap_id);
      }
    }
    parent_->data_.constraints = std::move(constraints);
    // Go through the remaining constraints to ensure we only delete scan
    // matchers of other submaps that have no inter-submap constraints left.
    // 检查剩余的约束以确保我们只删除没有子图间约束的其他子图的扫描匹配器
    for (const Constraint& constraint : parent_->data_.constraints) {
    
    
      if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) {
    
    
        continue;
      } 
      // 只要other_submap_ids_losing_constraints内submap_id还存在其他的子图间约束
      // 就把这个子图id从other_submap_ids_losing_constraints中删除, 可以留着
      else if (other_submap_ids_losing_constraints.count(
                     constraint.submap_id)) {
    
    
        // This submap still has inter-submap constraints - ignore it.
        other_submap_ids_losing_constraints.erase(constraint.submap_id);
      }
    }
    // Delete scan matchers of the submaps that lost all constraints.
    // TODO(wohe): An improvement to this implementation would be to add the
    // caching logic at the constraint builder which could keep around only
    // recently used scan matchers.
    // Step: 3 删除这些子图id的匹配器
    for (const SubmapId& submap_id : other_submap_ids_losing_constraints) {
    
    
      parent_->constraint_builder_.DeleteScanMatcher(submap_id);
    }
  }

  // Mark the submap with 'submap_id' as trimmed and remove its data.
  CHECK(parent_->data_.submap_data.at(submap_id).state ==
        SubmapState::kFinished);
  // Step: 4 删除这个子图的指针
  parent_->data_.submap_data.Trim(submap_id);
  // Step: 5 删除这个子图的匹配器, 与多分辨率地图
  parent_->constraint_builder_.DeleteScanMatcher(submap_id);
  // Step: 6 删除optimization_problem_中的这个子图
  parent_->optimization_problem_->TrimSubmap(submap_id);

  // We have one submap less, update the gauge metrics.
  kDeletedSubmapsMetric->Increment();
  if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) {
    
    
    kFrozenSubmapsMetric->Decrement();
  } else {
    
    
    kActiveSubmapsMetric->Decrement();
  }

  // Remove the 'nodes_to_remove' from the pose graph and the optimization
  // problem.
  // Step: 7 删除节点
  for (const NodeId& node_id : nodes_to_remove) {
    
    
    parent_->data_.trajectory_nodes.Trim(node_id);
    parent_->optimization_problem_->TrimTrajectoryNode(node_id);
  }
}

六、结语

通过该篇博客，对于 Cartographer 纯定位模式有了更加深入的了解，其主要核心就是对子图的修剪，步骤如下：

$\color{red} 核心$ max_submaps_to_keep 为3则表示最多存在三个子图，如果新建了第四个子图，则需要把最前面的子图删除掉。删除一个子图，那么就要删除与该子图相关的约束(子图内、子图间)、以及该子图对应的多分辨率地图、分支定界扫面匹配器等，最后才删除该地图的尚格数据等。