“vector”: 不是“std”的成员_Cartographer源码阅读16—PoseGraph成员函数2

上一篇：Cartographer源码阅读15—PoseGraph的成员函数

下一篇：暂无，敬请期待

---

这篇文章本应继续更新PoseGraph的成员函数的解读。但是这周末出国的飞机，我得收拾行李，好巧不巧又赶上这阵子重感冒，所以这周暂时停止更新。等我恢复元气了继续更新。谢谢大家的关注！——20181129

刚到国外，这几周忙着办理各种手续并熟悉环境之中，所以没能按时给大家更新。没想到这段时间专栏的关注人数已经增长了一倍，非常欣慰。也有一些同学在催更了，正好这里马上要到圣诞节和元旦假期了，所以先立一个flag，趁这个假期继续更新。——20181221

在上一篇文章中我们已经解读了PoseGraph2D::AddNode函数以及该函数所依赖的AddTrajectoryIfNeeded、AddWorkItem、ComputeConstraintsForNode，以及ComputeConstraintsForNode所依赖的InitializeGlobalSubmapPoses、ComputeConstraint和ComputeConstraintsForOldNodes (该函数调用了ComputeConstraint)等函数。

我们接着看下面两个函数：DispatchOptimization和GetLatestNodeTime。DispatchOptimization在ComputeConstraintsForNode中被调用，而GetLatestNodeTime在ComputeConstraint中被调用。

6. DispatchOptimization()

// Dispatch的含义是分拨、派遣、分拣。这里的主要是将run_loop_closure_置为true,
// 从而使得执行优化操作的代码块知道应该进行全局优化了。
void PoseGraph2D::DispatchOptimization() {
  run_loop_closure_ = true;
  // If there is a 'work_queue_' already, some other thread will take care.
  //如果工作队列还为空指针，需要创建一个工作队列
  if (work_queue_ == nullptr) {
  	//创建一个工作队列的指针
    work_queue_ = common::make_unique<std::deque<std::function<void()>>>();
    //为该工作队列绑定处理函数，即HandleWorkQueue
    constraint_builder_.WhenDone(
        std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
  }
}

7. GetLatestNodeTime

// 获取该node和该submap中的node中较新的时间
common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
                                            const SubmapId& submap_id) const {
  // 获取指定id的时间
  common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
  // 获取指定id的submap的数据
  const InternalSubmapData& submap_data = submap_data_.at(submap_id);
  if (!submap_data.node_ids.empty()) {//如果该submap相关的node_ids不为空。
  	// 获取指定id的submap相关的node_ids列表中的最后一个元素
    const NodeId last_submap_node_id =
        *submap_data_.at(submap_id).node_ids.rbegin();//c.rbegin() 返回一个逆序迭代器，它指向容器c的最后一个元素
    // 把时间更新为节点建立时间与submap中最后一个节点时间中较晚的那个
    time = std::max(
        time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
  }
  return time;
}

之后我们依据http://pose_graph_2d.cc中的顺序来依次解读一下各个函数的功能，基本上完成这些函数的解读后我们对Cartographer源码的解读也就基本告成。

8. AddImuData、AddOdometryData、AddFixedFramePoseData、AddLandmarkData

这几个函数的主要作用是在有新的传感器数据传入时进行处理：

void PoseGraph2D::AddImuData(const int trajectory_id,
                             const sensor::ImuData& imu_data) {
  common::MutexLocker locker(&mutex_);//设置互斥锁
  AddWorkItem([=]() REQUIRES(mutex_) {
    optimization_problem_->AddImuData(trajectory_id, imu_data);
  });
}

void PoseGraph2D::AddOdometryData(const int trajectory_id,
                                  const sensor::OdometryData& odometry_data) {
  common::MutexLocker locker(&mutex_);
  AddWorkItem([=]() REQUIRES(mutex_) {
    optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
  });
}

可以看出，IMU和里程计数据有新的测量结果后是调用optimization_problem_进行处理，该函数的处理目标也是优化机器人行进过程中的Node处的位姿以及Submap的绝对位姿，使其更好地符合观测结果。具体的处理方法等我们深入解读OptimizationProblem时再研究。

void PoseGraph2D::AddFixedFramePoseData(
    const int trajectory_id,
    const sensor::FixedFramePoseData& fixed_frame_pose_data) {
  LOG(FATAL) << "Not yet implemented for 2D.";
}

AddFixedFramePoseData是处理GPS等能够测量机器人完整位姿的传感器输入，目前2D情况下并没有用到该函数。

// 添加Landmark数据
void PoseGraph2D::AddLandmarkData(int trajectory_id,
                                  const sensor::LandmarkData& landmark_data)
    EXCLUDES(mutex_) {
  common::MutexLocker locker(&mutex_);
  AddWorkItem([=]() REQUIRES(mutex_) {
    for (const auto& observation : landmark_data.landmark_observations) {//遍历传入的参数中的landmark_data
      //根据数据生成一个LandmarkNode::LandmarkObservation，压入landmark_nodes_这个容器中
      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});
    }
  });
}

AddLandmarkData则是处理有Landmark的Observation的情况。Landmark的处理与IMU和里程计数据的处理不太一样。对于IMU和里程计数据而言，他们的局部信息是比较可靠的，所以在每次有新的观察结果时立刻交给OptimizationProblem进行处理。但对于Landmark的数据而言，cartographer认为Landmark的绝对位姿是已知的，而机器人在行进过程中可能会多次观察landmark，因此，landmark并不是有观测后就立刻处理，而是在进行Loop Closure之前才调用，在调用时可能观察到多次，遍历这些观测数据，并把他们放到landmark_nodes_这个容器中，然后在Loop Closure时一起优化。

由于我已经开始有新的科研任务了，所以对cartographer源码的解读只能利用节假日或其他休息时间进行更新。与之前全职工作时相比，更新速度肯定会下降很多，请大家多多见谅。

20190929：继续更新PoseGraph2D的源码解读。发现长时间不看，很多内容都已经忘了，还得重头开始看。

9. UpdateTrajectoryConnectivity(const Constraint& constraint)

UpdateTrajectoryConnectivity(const Constraint& constraint)是在每有一条新的约束constraint加入时，用来更新不同trajectory之间的connectivity的关系。所以，每当有新的约束加入时都会调用该函数更新地图中的trajectory之间的联系。

// 在每有一条新的约束constraint加入时，用来更新不同trajectory之间的connectivity的关系。
void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
  //检查constraint的标签类型是否是INTER_SUBMAP类型.
  CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
  //根据constraint中产生约束的一对儿node_id和submap_id获取两者上次建立约束的最晚时间
  const common::Time time =
      GetLatestNodeTime(constraint.node_id, constraint.submap_id);
  //对两者所在trajectory_id建立connectivity
  trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
                                         constraint.submap_id.trajectory_id,
                                         time);
}

10. HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)

该函数的输入是由ConstraintBuilder2D建立约束之后的结果。所以，该函数的主要工作就是处理这些建立起来的约束关系。这里我们也可以对Cartographer的全局优化有个大致的框架：PoseGraph2D中存在一个constraints::ConstraintBuilder2D类型的变量constraint_builder_, 它的主要任务就是在新的Node加入时建立起该Node与Submap之间的约束。把这些约束放到一个向量std::vector<Constraint> constraints_中。在满足一定条件时，由OptimizationProblem2D类型的变量optimization_problem_来进行全局优化。

// 输入是由ConstraintBuilder2D建立起来的约束向量
void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result) {
  {
    // 在处理数据时加上互斥锁，防止出现数据访问错误
    common::MutexLocker locker(&mutex_);
    // 把result中的所有约束加入到constraints_向量的末尾处
    constraints_.insert(constraints_.end(), result.begin(), result.end());
  }
  // 执行优化。这里调用了PoseGraph2D的另一个成员函数RunOptimization()来处理
  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();
      // 把optimization_problem_中的node和submap的数据拷贝到两个参数中
      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);
  }

  // 更新trajectory之间的connectivity信息
  common::MutexLocker locker(&mutex_);
  for (const Constraint& constraint : result) {
    UpdateTrajectoryConnectivity(constraint);
  }
  // 调用trimmers_中每一个trimmer的Trim函数进行处理。但还是不清楚这个trimming的含义是什么。
  TrimmingHandle trimming_handle(this);
  for (auto& trimmer : trimmers_) {
    trimmer->Trim(&trimming_handle);
  }
  // Trim之后把他们从trimmers_这个向量中清除，trimmers_将重新记录等待新一轮的Loop Closure过程中产生的数据
  trimmers_.erase(
      std::remove_if(trimmers_.begin(), trimmers_.end(),
                     [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
                       return trimmer->IsFinished();
                     }),
      trimmers_.end());

  // 重新把“上次Loop Closure之后新加入的节点数”置为0，run_loop_closure置为false
  num_nodes_since_last_loop_closure_ = 0;
  run_loop_closure_ = false;
  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();
  // We have to optimize again.
  // 调用constraint_builder_的WhenDone函数进一步处理
  constraint_builder_.WhenDone(
      std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));
}

---

上一篇：Cartographer源码阅读15—PoseGraph的成员函数

下一篇：暂无，敬请期待