【移动机器人技术】cartographer中pose_graph的work_queue_分析

在cartographer的定位过程中，基于位姿图的优化运算非常消耗资源，为了不影响系统的运行实时性，每部采用了任务队列管理机制，可以将优化运算任务压进队列，利空空闲的片段时间分批计算。本文描述了基本机制原理，供更好理解cartographer的整体运行机理。

1、work_queue_定义

在pose_graph_2d.h文件中，定义如下：

// If it exists, further work items must be added to this queue, and will be
// considered later.
std::unique_ptr<std::deque<std::function<void()>>> work_queue_
    GUARDED_BY(mutex_);

2、work_queue_创建

在pose_graph_2d.cc文件的void PoseGraph2D::DispatchOptimization() 函数中：

  if (work_queue_ == nullptr) {
    // 如果没有创建work_queue_，则先创建队列，再执行回环检测的任务；
    // 此处仅进入一次，后面就依靠run_loop_closure_标志，在HandleWorkQueue()函数中循环执行；
    work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
    
	...
  }
}

3、 work_queue_的添加

利用如下函数实现

void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
  if (work_queue_ == nullptr) {
    work_item();   // 如果没有队列，则直接执行任务函数
  } else {
    work_queue_->push_back(work_item);   // 如果有队列，则将任务追加到队列尾部
  }
}

如果没有创建队列，则直接执行任务函数；
如果已经创建队列，则将任务追加到队列尾部。

4、work_queue_的执行

在pose_graph_2d.cc文件的void PoseGraph2D::HandleWorkQueue()函数中：

 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();

当不执行回环检测时，若队列不为空，则执行队列首个任务，执行完毕后，将其从队列中删掉。
当需要运行回环检测时，执行完毕当前任务，跳出循环，打印任务任务个数信息。

5、work_queue_的任务

有哪些任务需要使用work_queue_机制呢？列些如下：

(1) 计算node约束

ComputeConstraintsForNode()

AddWorkItem([=]() REQUIRES(mutex_) {
    ComputeConstraintsForNode(node_id, insertion_submaps,
                              newly_finished_submap);
  });

(2)优化中增加传感器数据

• IMU数据optimization_problem_->AddImuData()

AddWorkItem([=]() REQUIRES(mutex_) {
    optimization_problem_->AddImuData(trajectory_id, imu_data);
  });

• 里程计数据optimization_problem_->AddOdometryData()

  AddWorkItem([=]() REQUIRES(mutex_) {
    optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
  });

• 增加地标数据（有点复杂，是一组地标数据）

AddWorkItem([=]() REQUIRES(mutex_) {
    for (const auto& observation : landmark_data.landmark_observations) {
      landmark_nodes_[observation.id].landmark_observations.emplace_back(
          LandmarkNode::LandmarkObservation{
              trajectory_id, landmark_data.time,
              observation.landmark_to_tracking_transform,
              observation.translation_weight, observation.rotation_weight});
    }
  });

（3）结束建图轨迹任务

执行结束建图轨迹后的优化：结束建图后，要将最后的采集到的submap数据进行回环优化，确保后面新增数据的精度。

AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
    CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
    finished_trajectories_.insert(trajectory_id);

    for (const auto& submap : submap_data_.trajectory(trajectory_id)) {
      submap_data_.at(submap.id).state = SubmapState::kFinished;
    }
    CHECK(!run_loop_closure_);
    DispatchOptimization();
  });

(4) 冻结轨迹FreezeTrajectory

AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
    CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0);
    frozen_trajectories_.insert(trajectory_id);
  });

(5)AddSubmapFromProto

AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
  submap_data_.at(submap_id).state = SubmapState::kFinished;
  optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
});

(6)AddNodeFromProto

AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
  const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
      constant_data->gravity_alignment.inverse());
  optimization_problem_->InsertTrajectoryNode(
      node_id,
      optimization::NodeSpec2D{
          constant_data->time,
          transform::Project2D(constant_data->local_pose *
                               gravity_alignment_inverse),
          transform::Project2D(global_pose * gravity_alignment_inverse),
          constant_data->gravity_alignment});
});

(7)AddNodeToSubmap

AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
    submap_data_.at(submap_id).node_ids.insert(node_id);
  });