cartographer PoseGraph2D_cartographer使用一张子图-CSDN博客

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/81317649

所处：

LocalTrajectoryBuilder2D cartographer/mapping/internal/2d/pose_graph_2d.cc /.h中

LocalTrajectoryBuilder2D 类在 map_build.cc 中 AddTrajectoryBuilder 函数中被构造

主要函数：

1. AddNode

在 GlobalTrajectoryBuilder类 AddSensorData 函数中调用

NodeId PoseGraph2D::AddNode(    //globalTrajectoryBuild
    std::shared_ptr<const TrajectoryNode::Data> constant_data,
    const int trajectory_id,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)

函数具体分析：

1、得到最优pose，将trajectory_id下的TrajectoryNode中局部位姿转换到世界坐标系下

  const transform::Rigid3d optimized_pose(
      GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);

2、生成的ConstanData的基础上，生成一个路径节点，并把这个节点保存在轨迹trajectory_nodes_中。

函数为 AppendNode（constant_data, trajectory_id,insertion_submaps, optimized_pose）

3、求局部变量 const bool newly_finished_submap（判断新子图是否完成）

4、AddWorkItem 添加工作组用于添加工作的队列work_queue_

5、计算约束为node ComputeConstraintsForNode(node_id, insertion_submaps,newly_finished_submap)

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)

函数具体分析：

1、如果需要时添加轨迹 AddTrajectoryIfNeeded(trajectory_id)

2、生成Node_id

  const NodeId node_id = data_.trajectory_nodes.Append(
      trajectory_id, TrajectoryNode{constant_data, optimized_pose});
  ++data_.num_trajectory_nodes;

3、记录轨迹中node的数加加 ++data_.num_trajectory_nodes

4、如果submap(size -1)没有分配下标，则为它分配下表

如果刚刚被插入的那个submap是我们之前从未看过的，就更新submap_data_的数据

也就是说submap_data_里面,包含了目前为止所有的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 << ".";
  }

3.ComputeConstraintsForNode

为激光数据帧scan计算约束为这帧新激光添加约束并开始在后台进行扫描匹配

WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap)

1、定义一些变量

  std::vector<SubmapId> submap_ids;
  std::vector<SubmapId> finished_submap_ids;
  std::set<NodeId> newly_finished_submap_node_ids;

获取node_id 的数据

    const auto& constant_data =
        data_.trajectory_nodes.at(node_id).constant_data;

2、初始化全局子图位姿将插入的submaps位姿全局化 InitializeGlobalSubmapPoses

 submap_ids = InitializeGlobalSubmapPoses(
        node_id.trajectory_id, constant_data->time, insertion_submaps);

3、计算匹配子图的id matching_id = submap_ids.front();

4、计算局部位姿

    const transform::Rigid2d local_pose_2d =
        transform::Project2D(constant_data->local_pose *
                             transform::Rigid3d::Rotation(
                                 constant_data->gravity_alignment.inverse()));

5、计算全局位姿当前子图的全局位姿 * 机器人在当前子图的相对位姿

    const transform::Rigid2d global_pose_2d =
        optimization_problem_->submap_data().at(matching_id).global_pose *
        constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
        local_pose_2d;

6、添加优化节点

    optimization_problem_->AddTrajectoryNode(
        matching_id.trajectory_id,
        optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                                 global_pose_2d,
                                 constant_data->gravity_alignment});

7、遍历所有插入子图（活跃子图），为每个子图与当前机器人加上约束。计算约束转换为：ComputeSubmapPose（子图位子求逆 *当前局部位姿）然后将约束push_back进去

枚举执行了插入操作的submap，即submap(size-1) & submap(size-2) 并为当前激光添加观测约束（因为当前激光帧刚刚匹配完成并插入到这两个submap中，所以是观测约束）Constraint2D形式的约束都是观测约束

    for (size_t i = 0; i < insertion_submaps.size(); ++i) {
      const SubmapId submap_id = submap_ids[i];
      // Even if this was the last node added to 'submap_id', the submap will
      // only be marked as finished in 'data_.submap_data' further below.
      CHECK(data_.submap_data.at(submap_id).state ==
            SubmapState::kNoConstraintSearch);
      data_.submap_data.at(submap_id).node_ids.emplace(node_id);

      //更新submap_id里面的node
      const transform::Rigid2d constraint_transform =
          constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
          local_pose_2d;

      //构建2D约束（所有的约束都是指观测值即观测约束）
      data_.constraints.push_back(
          Constraint{submap_id,
                     node_id,
                     {transform::Embed3D(constraint_transform),
                      options_.matcher_translation_weight(),
                      options_.matcher_rotation_weight()},
                     Constraint::INTRA_SUBMAP});
    }

8、如果子图的状态完成，则跟新子图已完成。

    for (const auto& submap_id_data : data_.submap_data) {
      if (submap_id_data.data.state == SubmapState::kFinished) {
        CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
        finished_submap_ids.emplace_back(submap_id_data.id);
      }
    }

9、枚举所有的submap 用当前的node/scan和所有finished的submap进行匹配来进行回环检测

  for (const auto& submap_id : finished_submap_ids) {
    ComputeConstraint(node_id, submap_id);
  }

10、我们有一个新完成的子图，所以我们考虑添加约束为旧节点 ComputeConstraint

  if (newly_finished_submap) {
    const SubmapId newly_finished_submap_id = submap_ids.front();
    // We have a new completed submap, so we look into adding constraints for
    // old nodes.
    for (const auto& node_id_data : optimization_problem_->node_data()) {
      const NodeId& node_id = node_id_data.id;
      if (newly_finished_submap_node_ids.count(node_id) == 0) {
        ComputeConstraint(node_id, newly_finished_submap_id);
      }
    }
  }

11、++节点数当节点数大于 optimize_every_n_nodes 时返回 WorkItem::Result::kRunOptimization

  if (options_.optimize_every_n_nodes() > 0 &&
      num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
    return WorkItem::Result::kRunOptimization;
  }

4.ComputeConstraint

void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
                                    const SubmapId& submap_id)

参数：node_id submap_id

1、定义变量 bool 添加局部、全局约束

  bool maybe_add_local_constraint = false;
  bool maybe_add_global_constraint = false;

3、计算node_time 和 last_connection_time

const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
const common::Time last_connection_time =
    data_.trajectory_connectivity_state.LastConnectionTime(
        node_id.trajectory_id, submap_id.trajectory_id);

4、确定增加全局约束还是局部约束

    if (node_id.trajectory_id == submap_id.trajectory_id ||
        node_time <
            last_connection_time +
                common::FromSeconds(
                    options_.global_constraint_search_after_n_seconds())) {
      // If the node and the submap belong to the same trajectory or if there
      // has been a recent global constraint that ties that node's trajectory to
      // the submap's trajectory, it suffices to do a match constrained to a
      // local search window.
      maybe_add_local_constraint = true;
    } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
      maybe_add_global_constraint = true;
    }

5、如果添加局部约束计算相对位姿，添加约束

constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, constant_data, initial_relative_pose);

6、如果添加全局约束

constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id,constant_data);

5.HandleWorkQueue //进行优化．

void PoseGraph2D::HandleWorkQueue(
    const constraints::ConstraintBuilder2D::Result& result)

1、添加约束结束（带互斥锁）

    absl::MutexLock locker(&mutex_);
    data_.constraints.insert(data_.constraints.end(), result.begin(),
                             result.end());

2、RunOptimization(); 运行优化函数

3、如果全局优化回调

4、更新轨迹连接

5、DrainWorkQueue()

6.RunOptimization

1、没有其他线程正在访问optimization_problem_，data_.constraints，data_.frozen_trajectories和data_.landmark_nodes
执行Solve时解决是耗时的，所以不要使用互斥锁在Solve之前避免阻止前台处理。

optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                               data_.landmark_nodes);
absl::MutexLock locker(&mutex_);

2、获取优化子图数据和节点数据

  const auto& submap_data = optimization_problem_->submap_data();
  const auto& node_data = optimization_problem_->node_data();

3、推断未包含在内的所有点云姿势‘optimization_problem_'。

4、计算新的局部位姿到全局位姿，并且跟新node数据

5、跟新优化后的landmark位姿

7.DrainWorkQueue

判断工作队列是否结束，判断是否进行优化