Cartographer源码阅读07-pose_graph 后端优化

前端优化告一段落，接下来我们对回环检测和后端优化的代码进行阅读。

pose_graph实际是SPA的一个实现,拓展成了submaping版本,每一个节点都会和一个或者多个子图进行匹配.每一个匹配都会形成一个约束,进行优化的时候节点的位姿和子图的位姿会一起进行优化. 所有的约束都是子图和节点之间的约束.
cartographer\mapping\internal\2d\pose_graph_2d.cc

1.AddNode

回顾源码阅读的第一篇文章，我们可以看到，在前端匹配完成后，调用InsertToSubmap函数，将激光数据插入匹配完成的子图之后，就开始调用pose_graph的AddNode函数来进行回环检测和后端优化。

AddNode的主要工作是把一个TrajectoryNode的数据加入到PoseGraph维护的trajectory_nodes_这个容器中。并计算跟这个节点相关的约束.然后返回node_id.

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)
{
  //该节点的全局位姿.
  //这个Pose是由trajectory相对于世界坐标系的Pose乘以
  //节点数据中包含的该节点相对于该trajectory的LocalTrajectoryBuilder 
  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

  //加入节点,并得到节点的id.
  const NodeId node_id = AppendNode(constant_data, trajectory_id,
                                    insertion_submaps, optimized_pose);

  // We have to check this here, because it might have changed by the time we
  // execute the lambda.
  // 该子图是否完成.即插入的帧数足够了.
  //当插入帧数到达一定数量时，子图就不会在接受激光数据的加入
  // 如果被finished，那么我们需要计算一下约束并且进行一下全局优化了。
  // 这里是把这个工作交到了workItem中等待处理
  const bool newly_finished_submap =
      insertion_submaps.front()->insertion_finished();

  //进行约束计算.--多线程.后台会调用ComputeConstraintsForNode()来计算约束.
  AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    return ComputeConstraintsForNode(node_id, insertion_submaps,
                                     newly_finished_submap);
  });

  return node_id;
}

• 计算关键帧全局位姿，并将此关键帧加入node队列，得到node_id，调用了AppendNode
• 查看插入子图的关键帧数量有没有达到上限，如果达到上限停止插入，开始计算约束，约束计算调用AddWorkItem
• 返回node_id

2.AppendNode-加入节点

NodeId PoseGraph2D::AppendNode(
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps,
    const transform::Rigid3d& optimized_pose)
{
  absl::MutexLock locker(&mutex_);
  AddTrajectoryIfNeeded(trajectory_id);
  if (!CanAddWorkItemModifying(trajectory_id))
  {
    LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
  }

  //加入当前轨迹中.
  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});

  ++data_.num_trajectory_nodes;

  // Test if the 'insertion_submap.back()' is one we never saw before.
  // 如果是新的submap,则需要加入到submap_中.
  if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
      std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
              ->data.submap != insertion_submaps.back())
  {
    // We grow 'data_.submap_data' as needed. This code assumes that the first
    // time we see a new submap is as 'insertion_submaps.back()'.
    const SubmapId submap_id =
        data_.submap_data.Append(trajectory_id, InternalSubmapData());

    data_.submap_data.at(submap_id).submap = insertion_submaps.back();

    LOG(INFO) << "Inserted submap " << submap_id << ".";
    kActiveSubmapsMetric->Increment();
  }

  return node_id;
}

直接加入节点队列，如果生成新的子图，同样将子图加入队列

3.后端优化

3.1AddWorkItem

将一个函数的地址加入工作队列，相当于把一个函数加入工作队列，在后端等待调用。

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

//DrainWorkQueue()将指定的函数的地址依次取出

    task->SetWorkItem([this]() { DrainWorkQueue(); });
    
    thread_pool_->Schedule(std::move(task));
  }

  //将函数加入队列中.work_item相当于指向函数的指针
  const auto now = std::chrono::steady_clock::now();
  work_queue_->push_back({now, work_item});

  kWorkQueueDelayMetric->Set(
      std::chrono::duration_cast<std::chrono::duration<double>>(
          now - work_queue_->front().time)
          .count());
}

重要步骤：调用DrainWorkQueue将指定函数取出，然后将压入工作队列

3.2 DrainWorkQueue

从work_queue_中不断的取函数ComputeConstraintsForNode()出来进行计算，至少返回的结果是进行WorkItem::Result::kRunOptimization;为止

.这里的work_item()就是调用ComputeConstraintsForNode()函数.

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;
      }
      
      //不为空，则开始取出函数ComputeConstraintsForNode
      work_item = work_queue_->front().task;
      work_queue_->pop_front();
      work_queue_size = work_queue_->size();
    }

   // work_item实际是函数指针，指向ComputeConstraintsForNode
   //当work_item（）时，可以直接理解为实际就是ComputeConstraintsForNode（）
  
  //ComputeConstraintsForNode()返回两个结果，一个是需要优化，一个是不需要优化


    process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
  }
  LOG(INFO) << "Remaining work items in queue: " << work_queue_size;


  //到了这里说明需要进行优化了,ComputeConstraintsForNode()返回了需要进行优化的结果.

  // We have to optimize again.
  constraint_builder_.WhenDone(
      [this](const constraints::ConstraintBuilder2D::Result& result) {
        HandleWorkQueue(result);
      });
}

当需要优化时，最后调用了HandleWorkQueue（）函数来进行优化

3.3HandleWorkQueue

当ComputeConstraintsForNode()返回需要优化的结果后,调用本函数.

本函数会进行pose-graph的优化.在DrainWorkQueue()里面进行调用.
代码中对理清思路最重要的是：

• 进行优化：RunOptimization()
• 优化完成之后，继续处理剩下的workitem：DrainWorkQueue();

void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result)
{
  {
    absl::MutexLock locker(&mutex_);
    data_.constraints.insert(data_.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;
    {
      absl::MutexLock 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())
      {
        if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
            submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0)
        {
          continue;
        }

        trajectory_id_to_last_optimized_node_id.emplace(
            trajectory_id,
            std::prev(node_data.EndOfTrajectory(trajectory_id))->id);

        trajectory_id_to_last_optimized_submap_id.emplace(
            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);
  }

  {
     //更新图的联通性.
    absl::MutexLock locker(&mutex_);
    for (const Constraint& constraint : result)
    {
      UpdateTrajectoryConnectivity(constraint);
    }

    //对轨迹进行删除
    DeleteTrajectoriesIfNeeded();

    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_ = 0;

    // Update the gauges that count the current number of constraints.
    // 统计约束的数量.
    double inter_constraints_same_trajectory = 0;
    double inter_constraints_different_trajectory = 0;
    for (const auto& constraint : data_.constraints)
    {
      if (constraint.tag ==
          cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP)
      {
        continue;
      }
      if (constraint.node_id.trajectory_id ==
          constraint.submap_id.trajectory_id)
      {
        ++inter_constraints_same_trajectory;
      }
      else
      {
        ++inter_constraints_different_trajectory;
      }
    }
    kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
    kConstraintsDifferentTrajectoryMetric->Set(
        inter_constraints_different_trajectory);
  }

  //继续处理剩下的workitem.
  DrainWorkQueue();
}

3.4RunOptimization

正式进行优化的函数.被HandleWorkQueue()调用.

void PoseGraph2D::RunOptimization()
{
  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.
  // 正式进行求解.
  //当误差函数构造完成之后，直接调用ceres库求解
  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);

  absl::MutexLock 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())
  {
    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.
    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);

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

    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;
    }
  }

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

  data_.global_submap_poses_2d = submap_data;
}

小节

这部分的代码实现的功能就是将节点加入，并且计算约束（回环检测）以及后端优化。本篇文章大家看了后端优化的内容，就下一篇将介绍回环检测，也就是ComputeConstraintsForNode的内容。