cartographer 代码思想解读（18）- PoseGraph2D整体处理流程2

最新推荐文章于 2023-07-03 20:56:03 发布

jiajiading

最新推荐文章于 2023-07-03 20:56:03 发布

阅读量1.2k

点赞数 1

分类专栏： slam

本文链接：https://blog.csdn.net/jiajiading/article/details/110183867

版权

slam 专栏收录该内容

29 篇文章 90 订阅

订阅专栏

cartographer 代码思想解读（18）- PoseGraph2D整体处理流程2

后端（后台）任务压栈AddWorkItem
后台任务执行循环DrainWorkQueue
后端图优化任务管理函数HandleWorkQueue
优化函数RunOptimization
总结

接上篇。

后端（后台）任务压栈AddWorkItem

后端优化的操作基本都是采用线程池在后台运行，cartographer采用了一个个任务放入任务队列work_queue_中，然后排队调用执行。

// 将一个函数地址加入到一个工作队列中
void PoseGraph2D::AddWorkItem(
    const std::function<WorkItem::Result()>& work_item) {
  absl::MutexLock locker(&work_queue_mutex_);
  // 如果工作队列未被初始化，则先初始化并启动线程执行任务队列
  if (work_queue_ == nullptr) {
    work_queue_ = absl::make_unique<WorkQueue>();
    auto task = absl::make_unique<common::Task>();
    task->SetWorkItem([this]() { DrainWorkQueue(); });
    thread_pool_->Schedule(std::move(task));
  }
  // 将当前任务插入到工作队列中，等待执行
  const auto now = std::chrono::steady_clock::now();
  work_queue_->push_back({now, work_item});
  kWorkQueueSizeMetric->Set(work_queue_->size());
  kWorkQueueDelayMetric->Set(
      std::chrono::duration_cast<std::chrono::duration<double>>(
          now - work_queue_->front().time)
          .count());
}

将需要执行的任务放入work_queue_缓存队列中等待执行；在DrainWorkQueue()中将任务队列中的任务一个一个排队执行。如果work_queue_为空指针，表明未被初始化，即在增加第一个任务前，需要进行初始化，并且开启线程池，后台打开任务队列执行循环。

后台任务执行循环DrainWorkQueue

其主要功能从将需要执行的任务放入work_queue_中取出一个任务进行执行。

void PoseGraph2D::DrainWorkQueue() {
  bool process_work_queue = true;
  size_t work_queue_size;
  while (process_work_queue) {
    std::function<WorkItem::Result()> work_item;
    {
      absl::MutexLock locker(&work_queue_mutex_);
      // 工作队列已空，则可直接退出
      if (work_queue_->empty()) {
        work_queue_.reset();
        return;
      }
      // 获取队列中最前的任务
      work_item = work_queue_->front().task;
      work_queue_->pop_front();
      work_queue_size = work_queue_->size();
      kWorkQueueSizeMetric->Set(work_queue_size);
    }
    // 当前工作任务的状态
    process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
  }
  LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
  // We have to optimize again.
  // 继续执行优化
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
        HandleWorkQueue(result);
      });
}

一旦开始调用，开始死循环，不断从work_queue_队列中取出压栈的任务一个一个执行，直到任务队列为空。如果队列中出现需要进行执行优化操作的，则退出循环，进入约束优化操作HandleWorkQueue，并优化的结果的采用回调的方式返回到result中。注意：在整个SLAM中实时进入任务队列的主要为前面所提的计算约束，当经过一定间隔时返回约束则为需要优化计算，而间隔内仅计算约束无需进行优化；

后端图优化任务管理函数HandleWorkQueue

在任务执行时，即主要为计算约束，当返回的约束结果需要优化时，则需要执行优化操作。而优化的具体的实现在优化器optimization_problem_中，而HandleWorkQueue为顶层调用管理。

void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result) {
      // 将新的约束添加到全局约束队列中
  {
    absl::MutexLock locker(&mutex_);
    data_.constraints.insert(data_.constraints.end(), result.begin(),
                             result.end());
  }
  // 进行全局优化
  RunOptimization();

  //如果设置了全局优化回调函数，则进行调用
  // 回调函数结果传递，省略........

  {
    // 根据约束结果，更新轨迹间的链接关系
	//代码省略
	
    //闭环后累计节点进行清零
    //表明已经完成了一次闭环
    num_nodes_since_last_loop_closure_ = 0;

	// 代码省略
  }
// 重新开启任务队列
  DrainWorkQueue();
}

中间维护轨迹间关系代码和数据统计代码省略，代码主要展现主要操作流程。

执行RunOptimization();即进行优化解算。
每优化一次则将优化节点计数器进行清零，用于重新累计个数，需要优化的约束根据此计数器个数触发执行。
执行完成优化任务后，重新开启任务缓存队列循环功能，即返回继续执行workqueue_里的任务。

优化函数RunOptimization

// 进行优化处理，并将优化结果进行更新
void PoseGraph2D::RunOptimization() {
  // 如果submap为空直接退出
  if (optimization_problem_->submap_data().empty()) {
    return;
  }

  // No other thread is accessing the optimization_problem_,
  // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
  // when executing the Solve. Solve is time consuming, so not taking the mutex
  // before Solve to avoid blocking foreground processing.
  // 调用优化，由于耗时间，故没加锁，防止阻塞其他线程
  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);

根据优化器中的submap节点是否存在，如果不存在则无需进行优化。否则调用优化构建器执行优化操作。

absl::MutexLock locker(&mutex_);

//获取优化后的结果
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();

获取经优化后所有的submap和node数据。

for (const int trajectory_id : node_data.trajectory_ids())

遍历所有优化结果后的轨迹节点位姿。

    // 更新所有节点的全局位置
    for (const auto& node : node_data.trajectory(trajectory_id)) {
      auto& mutable_trajectory_node = data_.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);
    }

将已优化后的轨迹点位姿将原有存储的位置全部进行替换。

    // Extrapolate all point cloud poses that were not included in the
    // 'optimization_problem_' yet.
    // 获取submap的local到global的转移矩阵
    const auto local_to_new_global =
        ComputeLocalToGlobalTransform(submap_data, trajectory_id);
    // 获取过去的转移矩阵
    const auto local_to_old_global = ComputeLocalToGlobalTransform(
        data_.global_submap_poses_2d, trajectory_id);
    //获取old到新的转移矩阵
    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(data_.trajectory_nodes.find(last_optimized_node_id));
    //将后续未优化的节点的全局pose进行转移（其转移矩阵是根据已优化的位置转移量） 
    for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id);
         ++node_it) {
      auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
      mutable_trajectory_node.global_pose =
          old_global_to_new_global * mutable_trajectory_node.global_pose;
    }
  }

由于优化并非是实时，是在后台进行运行的，并且需要一定的时间。因此当完成优化时，前端输出结果已经对整个位姿图个数进行了增加。因此在开始进行优化时后面新加入的节点并未进行优化处理，因此返回优化的结果没有最新加入轨迹节点对应的结果。因此需要采用优化后结果中最后一个轨迹节点的位姿态的转移矩阵，作为未参与优化轨迹节点的转移矩阵进行更新。看参考以下，其中 $X$ 为参与优化的节点，优化前位姿为 $X_{old}$ ，优化后的位姿为 $X_{new}$ ，而 $Y$ 则为未参与优化的节点。

$T = X_{old}^{-1} * X_{new}$
$Y_{new} = T * Y_{old}$

  for (const auto& landmark : optimization_problem_->landmark_data()) {
    data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
  }

更新所有路标位姿。

  // 将优化后的submap进行替代原队列
  data_.global_submap_poses_2d = submap_data;

更新所有submap的位置。

总结

cartographer后端完成分析，其主要由posegraph2d类进行实现。其主要功能是收集轨迹位姿节点、所有submap，然后计算其约束，节点不是构成submap的约束计算闭环，从而构建完成的位姿图，最后调用ceres的非线性优化库实现优化，将最后优化的结果更新已存储的结果。按照slam的思想，前端匹配需要实时，但后端不需要实时，因此采用缓存在后台同步优化处理，无需暂停前端处理，多线程需要考虑锁保护功能。