[cartographer疑问系列] 1. 优化之后的recover

优化后的recover有两个目的：

1. rviz显示

2. for 下一次优化

具体的recover操作在pose_graph_2d.cc  的 RunOptimization 函数：

//正式进行优化的函数.
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.
  // 正式进行求解.
  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);
  absl::MutexLock locker(&mutex_);
  // 利用优化结果recover
  // 1. recover node
  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;
    }
  }

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

  // 3. recover submap
  data_.global_submap_poses_2d = submap_data;
}

注意：data_.global_submap_poses_2d = submap_data; 只是更新了 data中的子图全局位姿变量，for 显示；并未改变真正的 子图 变量，所以，在之后的前端中，依旧在未闭环的pose上进行match。不过，不必担心，因为前端得到的是local pose ， 进入优化器时 需要转换为 global pose 。 这里需要计算 local 2  global 的变换关系，其实就是最新的闭环误差。

for 优化：

     optimization_problem_ 内，在优化完成后，顶点的pose自动更新了

for 显示：

     在node.cc文件中，

     1）轨迹显示：

      PublishTrajectoryNodeList ----> GetTrajectoryNodeList ------>

pose_graph()->GetTrajectoryNodePoses() // 获得实时轨迹信息： pose graph 的 data.nodepose.globalpose ；即经过优化的全局位姿

      2)  子图显示：

             PublishSubmapList -------> pose_graph()->GetAllSubmapPoses()

               -------->  GetSubmapDataUnderLock

// 位于pose_graph_2d.cc 获得子图的pose 
PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
    const SubmapId& submap_id) const {
  const auto it = data_.submap_data.find(submap_id);
  if (it == data_.submap_data.end()) {
    return {};
  }
  auto submap = it->data.submap;

  // 如果所需子图经过优化了，则返回优化后的pose
  if (data_.global_submap_poses_2d.Contains(submap_id)) {
    // We already have an optimized pose.
    return {submap,
            transform::Embed3D(
                data_.global_submap_poses_2d.at(submap_id).global_pose)};
  }
  // 否则就返回未优化的pose
  // We have to extrapolate.
  return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
                                                submap_id.trajectory_id) *
                      submap->local_pose()};
}

下一步，咋rviz显示概率栅格图呢？概率值在哪里体现呢？