Cartographer代码阅读笔记：位姿图的创建与更新

本文将深入研究一下Cartographer是如何创建和位姿图的更新，相关文件在：cartographer/mapping/internal/2d/pose_graph_2d.cc中。该文件代码实现了将Local SLAM输出的子图信息和激光点云数据添加到位姿图中，并通过一个约束构建器在后端完成闭环检测。每当Local SLAM有输出的时候，PoseGraph2D不一定立即计算约束和更新位姿图的，而是会放入一个工作队列中，等待闭环检测结束后再调度工作队列完成更新操作。

Cartographer有关其他分析文章：回到目录

接下来探讨一下每个函数的具体实现过程：

PoseGraph2D()构造函数

/** 
 * @brief 位姿图构造函数，在构造列表中完成一些成员变量的构造
 * @param options 是从配置文件中加载的关于位姿图的配置项
 * @param optimization_problem 是一个智能指针指向后端优化问题求解器
 * @param thread_pool 是一个线程池
 */
PoseGraph2D::PoseGraph2D(
    const proto::PoseGraphOptions& options,
    std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
    common::ThreadPool* thread_pool)
    : options_(options),
      optimization_problem_(std::move(optimization_problem)),
      constraint_builder_(options_.constraint_builder_options(), thread_pool) {}

InitializeGlobalSubmapPoses()函数分析

/** 
 * @brief 获取新旧子图索引，同时检查了新子图是否第一次被后端看见，若是则为之计算全局位姿并喂给后端优化器optimization_problem_;
 * 在函数ComputeConstraintsForNode()中调用
 * @param trajectory_id 轨迹索引
 * @param time 调用AddNode时对应路径节点的时间戳
 * @param insertion_submaps 是从Local SLAM一路传递过来的新旧子图。
 * @return 记录insertion_submaps中各个子图分配的SubmapId。
 */
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  CHECK(!insertion_submaps.empty());

  // 获取后端优化器的子图位姿信息记录到临时对象submap_data中。
  const auto& submap_data = optimization_problem_->submap_data();

  // 如果insertion_sumbaps中只有一个子图，意味着重新开始了一条轨迹。
  if (insertion_submaps.size() == 1) {  
    // 检测后端优化器中是否已经存在一条索引为trajectory_id的轨迹，若没有则创建一个。
    if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {

      // 检查trajectory_id是否与之前的轨迹有关联，若有则根据initial_trajectory_poses_的描述构建连接关系。
      if (initial_trajectory_poses_.count(trajectory_id) > 0) {
        trajectory_connectivity_state_.Connect(
            trajectory_id,
            initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
      }

      // 通过后端优化器的接口AddSubmap创建一条新的轨迹，并将子图的全局位姿信息(id 和 pose)喂给优化器。
      optimization_problem_->AddSubmap(
          trajectory_id,
          transform::Project2D(ComputeLocalToGlobalTransform(
                                   global_submap_poses_, trajectory_id) *
                               insertion_submaps[0]->local_pose()));
    }

    // 检查数据关系，为新建的子图赋予唯一的SubmapId，并返回之。
    CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
    const SubmapId submap_id{trajectory_id, 0};
    CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
    return {submap_id};
  }

  // 函数控制流走到这，说明trajectory_id下已经至少有了一个子图，
  // 此时输入的insertion_submaps中应当有两个子图，也就是Local SLAM中维护的新旧子图。
  CHECK_EQ(2, insertion_submaps.size());
  const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
  CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);

  // 用last_submap_id记录后端优化器中最新子图的索引。
  const SubmapId last_submap_id = std::prev(end_it)->id;

  // 根据last_submap_id检查后端优化器中最新的子图是否与insertion_submaps中的旧图是同一个对象。
  // 若是，则说明新图是Local SLAM新建子图的后端尚未记录。
  if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {   // 0
    // In this case, 'last_submap_id' is the ID of
    // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
    // 将新图的全局位姿提供给后端优化器，并分配一个SubmapId。
    const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
    optimization_problem_->AddSubmap(
        trajectory_id,
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1]));

    // 将新旧子图的SubmapId放到容器中一并放回
    return {last_submap_id,
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
  }

  // 最后一种情况，Local SLAM并没有再新建子图，此时后端中记录了所有的子图，只需要将新旧子图对应的SubmapId返回即可。
  CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back());
  const SubmapId front_submap_id{trajectory_id,
                                 last_submap_id.submap_index - 1};
  CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front());
  return {front_submap_id, last_submap_id};
}

AddTrimmer()函数分析

/** 
 * @brief 给位姿图对象添加OverlappingSubmapsTrimmer2D类型和PureLocalizationTrimmer类型的修饰器，分别用于根据子图间重叠部分修饰地图和纯粹定位，该函数在MapBuilder::AddTrajectoryBuilder中被调用。
 * @param trimmer 是一个智能指针指向OverlappingSubmapsTrimmer2D类型和PureLocalizationTrimmer类型的修饰器
 */
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
  common::MutexLocker locker(&mutex_);
  PoseGraphTrimmer* const trimmer_ptr = trimmer.release();

  // 参数是一个lambda表达式，该表达式不会立即被执行，将被当做类似函数指针或仿函数的可执行对象传给函数AddWorkItem,并在合适的条件下被调用
  AddWorkItem([this, trimmer_ptr]()
                  REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
}

AddWorkItem()函数分析

/** 
 * @brief 任务添加到工作队列
 * @param work_item 任务执行函数
 */
void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
  // 如果工作队列不存在，则直接运行任务函数，否则放到队列中等待以后执行
  if (work_queue_ == nullptr) {
    work_item();
  } else {
    work_queue_->push_back(work_item);
  }
}

SetInitialTrajectoryPose()函数分析

/** 
 * @brief 完成初始位姿配置
 * @param from_trajectory_id 新轨迹索引
 * @param to_trajectory_id 参考轨迹索引
 * @param pose 新轨迹相对参考轨迹的位姿
 * @param time 时间戳
 */
void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
                                           const int to_trajectory_id,
                                           const transform::Rigid3d& pose,
                                           const common::Time time) {
  common::MutexLocker locker(&mutex_);

  // 创建一个InitialTrajectoryPose类型对象，放置到容器initial_trajectory_poses_中
  initial_trajectory_poses_[from_trajectory_id] =
      InitialTrajectoryPose{to_trajectory_id, pose, time};
}

AddNode()函数分析

/** 
 * @brief 添加来自Local SLAM的子图和轨迹节点数据到容器对象submap_data_和trajectory_nodes中；
 * 并通过工作队列调用函数ComputeConstraintsForNode计算轨迹节点和子图之间的约束关系。
 * @param constant_data 记录了更新子图时的点云信息和相对位姿
 * @param trajectory_id 记录了轨迹索引
 * @param insertion_submaps 是更新的子图
 * @return 轨迹节点索引
 */
NodeId PoseGraph2D::AddNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  // 获得局部（点云）坐标系到世界坐标系的坐标变换关系，
  // 函数GetLocalToGlobalTransform是根据最新一次优化之后的子图位姿生成局部（子图）坐标系到世界坐标系的坐标变换关系
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  // 对信号量加锁，保证多线程的安全
  common::MutexLocker locker(&mutex_);

  // 维护各个轨迹之间的连接关系
  AddTrajectoryIfNeeded(trajectory_id);

  // 根据输入constant_data数据和刚刚计算的坐标变换构建一个TrajectoryNode的对象，
  // 并将该对象添加到轨迹节点容器trajectory_nodes_中，同时分配一个NodeId
  const NodeId node_id = trajectory_nodes_.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});
  ++num_trajectory_nodes_;

  // 判断容器submap_data_中是否有轨迹索引为trajectory_id的子图，再判定insertion_submaps.back()中的子图是否是最新生成的。
  // 若是则将之添加到容器submap_data_中，同时分配一个SubmapId
  if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
          insertion_submaps.back()) {
    const SubmapId submap_id =
        submap_data_.Append(trajectory_id, InternalSubmapData());
    submap_data_.at(submap_id).submap = insertion_submaps.back();
  }

  // 查询子图是否更新完成，即插入的帧数是否足够。
  const bool newly_finished_submap = insertion_submaps.front()->finished();

  // 通过lambda表达式和函数AddWorkItem注册一个为新增节点添加约束的任务ComputeConstraintsForNode()。
  // 根据Cartographer的思想，该任务会将新增的节点与所有已处于kFinished状态的子图进行一次匹配建立可能存在的闭环约束。
  AddWorkItem([=]() REQUIRES(mutex_) {
    ComputeConstraintsForNode(node_id, insertion_submaps,
                              newly_finished_submap);
  });
  return node_id;
}

ComputeConstraintsForNode()函数分析

/** 
 * @brief 添加子图和节点之间的约束，同时会触发工作队列运行
 * @param node_id 记录了待更新节点的索引
 * @param insertion_submaps 记录了从Local SLAM一路传递过来的新旧子图
 * @param newly_finished_submap 记录了新旧子图是否结束更新
 */
void PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {

  // 获取该节点的数据。
  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;

  // 获取新旧子图的索引
  const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
      node_id.trajectory_id, constant_data->time, insertion_submaps);
  CHECK_EQ(submap_ids.size(), insertion_submaps.size());
  
  // 获取旧图SubmapId
  const SubmapId matching_id = submap_ids.front();

  // 获取节点相对于子图的位姿
  const transform::Rigid2d local_pose_2d = transform::Project2D(
      constant_data->local_pose *
      transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));

  // 获取节点在世界坐标系下的位姿      
  const transform::Rigid2d global_pose_2d =
      optimization_problem_->submap_data().at(matching_id).global_pose *
      constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
      local_pose_2d;

  // 将相关数据提供给后端优化器optimization_problem_      
  optimization_problem_->AddTrajectoryNode(
      matching_id.trajectory_id,
      optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                               global_pose_2d,
                               constant_data->gravity_alignment});
  
  // 为新增的节点和新旧子图之间添加INTRA_SUBMAP类型的约束（INTER_SUBMAP约束在ConstraintBuilder2D::ComputeConstraint()函数中添加）                            
  for (size_t i = 0; i < insertion_submaps.size(); ++i) {
    const SubmapId submap_id = submap_ids[i];
    // Even if this was the last node added to 'submap_id', the submap will
    // only be marked as finished in 'submap_data_' further below.
    CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
    submap_data_.at(submap_id).node_ids.emplace(node_id);
    const transform::Rigid2d constraint_transform =
        constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
        local_pose_2d;
    constraints_.push_back(Constraint{submap_id,
                                      node_id,
                                      {transform::Embed3D(constraint_transform),
                                       options_.matcher_translation_weight(),
                                       options_.matcher_rotation_weight()},
                                      Constraint::INTRA_SUBMAP});
  }

  // 遍历所有子图，找到已经处于kFinished状态的子图，建立他们与新增节点之间可能的约束
  for (const auto& submap_id_data : submap_data_) {
    if (submap_id_data.data.state == SubmapState::kFinished) {
      CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
      ComputeConstraint(node_id,  submap_id_data.id);
    }
  }

  if (newly_finished_submap) {
    const SubmapId finished_submap_id = submap_ids.front();
    InternalSubmapData& finished_submap_data =
        submap_data_.at(finished_submap_id);
    CHECK(finished_submap_data.state == SubmapState::kActive);
    // 如果新图已经完成，把新图state设置为kFinished。
    finished_submap_data.state = SubmapState::kFinished;

    // 建立新图与旧图之间可能的约束关系，即进行回环检测。
    ComputeConstraintsForOldNodes(finished_submap_id);
  }

  // 约束计算完毕，通知约束构建器结束约束构建操作
  constraint_builder_.NotifyEndOfNode();
  ++num_nodes_since_last_loop_closure_;
  CHECK(!run_loop_closure_);

  // 当积累了一定数量的新节点后，会通过DispatchOptimization()函数将run_loop_closure_置为true,进而触发回环检测
  if (options_.optimize_every_n_nodes() > 0 &&
      num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
    DispatchOptimization();
  }
}

DispatchOptimization()函数分析

/** 
 * @brief 优化方法调度
 */
void PoseGraph2D::DispatchOptimization() {
  run_loop_closure_ = true;

  // 判断工作队列是否存在，如果不存在就创建一个
  if (work_queue_ == nullptr) {
    work_queue_ = common::make_unique<std::deque<std::function<void()>>>();

    // 通过约束构建器的WhenDone接口注册一个回调函数HandleWorkQueue
    constraint_builder_.WhenDone(
        std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
  }
}

HandleWorkQueue()函数分析

/** 
 * @brief 调度任务队列里的任务
 * @param result 记录了约束构建器完成后的结果
 */
void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result) {
  {
    // 将构建的约束添加到容器contraints_中
    common::MutexLocker locker(&mutex_);
    constraints_.insert(constraints_.end(), result.begin(), result.end());
  }

  // 进行优化
  RunOptimization();

  // 如果提供了全局优化之后的回调函数，则调用回调函数，默认是没有提供
  if (global_slam_optimization_callback_) {
    std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
    std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
    {
      common::MutexLocker locker(&mutex_);
      const auto& submap_data = optimization_problem_->submap_data();
      const auto& node_data = optimization_problem_->node_data();
      for (const int trajectory_id : node_data.trajectory_ids()) {
        trajectory_id_to_last_optimized_node_id[trajectory_id] =
            std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
        trajectory_id_to_last_optimized_submap_id[trajectory_id] =
            std::prev(submap_data.EndOfTrajectory(trajectory_id))->id;
      }
    }
    global_slam_optimization_callback_(
        trajectory_id_to_last_optimized_submap_id,
        trajectory_id_to_last_optimized_node_id);
  }

  common::MutexLocker locker(&mutex_);
  for (const Constraint& constraint : result) {
    // 更新轨迹之间的连接关系
    UpdateTrajectoryConnectivity(constraint);
  }

  // 调用修饰器完成地图的修饰
  TrimmingHandle trimming_handle(this);
  for (auto& trimmer : trimmers_) {
    trimmer->Trim(&trimming_handle);
  }
  trimmers_.erase(
      std::remove_if(trimmers_.begin(), trimmers_.end(),
                     [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                       return trimmer->IsFinished();
                     }),
      trimmers_.end());

  // 以完成一次地图的优化，将计数器num_nodes_since_last_loop_closure_清零
  num_nodes_since_last_loop_closure_ = 0;
  // 更新run_loop_closure_为false，表示当前系统空闲，没有进行回环检测
  run_loop_closure_ = false;

  // 循环中处理掉work_queue_中所有等待的任务，这些任务主要是添加节点、添加传感器数据到位姿图中
  while (!run_loop_closure_) {
    if (work_queue_->empty()) {
      work_queue_.reset();
      return;
    }
    work_queue_->front()();
    work_queue_->pop_front();
  }
  LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();

  // 有时还没有完全处理完队列中的所有任务，因为run_loop_closure_状态在DispatchOptimization()函数中置成true，
  // 开启新的回环检测而退出。此时需要重新注册一下回调函数。
  constraint_builder_.WhenDone(
      std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
}

RunOptimization()函数分析

/** 
 * @brief 进行优化调用的函数同时完成优化后新增节点的位姿调整，以适应优化后的世界地图和运动轨迹。
 * 由工作队列的函数HandleWorkQueue()调用
 */
void PoseGraph2D::RunOptimization() {
  // 首先检查是否给后端优化器喂过数据
  if (optimization_problem_->submap_data().empty()) {
    return;
  }

  // 通过后端优化器的接口Solve进行SPA优化。
  // 程序运行到这里的时候，实际上没有其他线程访问optimizaion_problem_,constraints_,
  // frozen_trajectories_和landmark_nodes_这四个对象。
  // 同时因为Solve这个接口太耗时了，所以没有在该函数之前加锁，以防止阻塞其他任务。
  optimization_problem_->Solve(constraints_, frozen_trajectories_,
                               landmark_nodes_);
   
  common::MutexLocker locker(&mutex_);
  // 获取后端优化器中子图和轨迹节点数据，分别记录在临时对象submap_data和node_data中
  const auto& submap_data = optimization_problem_->submap_data();
  const auto& node_data = optimization_problem_->node_data();
  // 遍历所有轨迹
  for (const int trajectory_id : node_data.trajectory_ids()) {
    // 遍历所有节点，用优化后的位姿来更新轨迹节点的世界坐标
    for (const auto& node : node_data.trajectory(trajectory_id)) {
      auto& mutable_trajectory_node = trajectory_nodes_.at(node.id);
      mutable_trajectory_node.global_pose =
          transform::Embed3D(node.data.global_pose_2d) *
          transform::Rigid3d::Rotation(
              mutable_trajectory_node.constant_data->gravity_alignment);
    }

    // 计算SPA优化前后的世界坐标变换关系
    const auto local_to_new_global =
        ComputeLocalToGlobalTransform(submap_data, trajectory_id);
    const auto local_to_old_global =
        ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
    const transform::Rigid3d old_global_to_new_global =
        local_to_new_global * local_to_old_global.inverse();
    // 将计算得到的坐标变换左乘在后来新增的轨迹节点的全局位姿上，得到修正后的轨迹
    const NodeId last_optimized_node_id =
        std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
    auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
    for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
         ++node_it) {
      auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id);
      mutable_trajectory_node.global_pose =
          old_global_to_new_global * mutable_trajectory_node.global_pose;
    }
  }

  // 更新路标位姿，并用成员变量global_submap_poses_记录当前的子图位姿。
  for (const auto& landmark : optimization_problem_->landmark_data()) {
    landmark_nodes_[landmark.first].global_landmark_pose = landmark.second;
  }
  global_submap_poses_ = submap_data;
}

返回顶部 or 回到目录