[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显示概率栅格图呢?概率值在哪里体现呢?

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值