Cartographer源码阅读08 回环检测计算约束

最新推荐文章于 2023-04-08 16:44:23 发布

你好哇zzz

最新推荐文章于 2023-04-08 16:44:23 发布

阅读量2.6k

点赞数 6

分类专栏： SLAM 文章标签：算法人工智能 c++ slam

本文链接：https://blog.csdn.net/weixin_45744060/article/details/107576680

版权

SLAM 专栏收录该内容

10 篇文章 13 订阅

订阅专栏

本篇文章，将就如何实现回环检测和计算约束来进行深一步的讨论。

代码位于cartographer\mapping\internal\2d\pose_graph_2d.cc

文章目录

1.回环检测

为Node进行约束计算：包括当前node_id跟所有submap的约束
和所有的node和insertion_submaps之间的约束
/调用ComputeConstraint()函数实现.

三个参数，一个是刚加入的节点ID；一个是Local Slam返回的insertion_submaps；一个是是否有新finished的submap的判断标志

WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,//刚加入的节点ID
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap)
{
  std::vector<SubmapId> submap_ids;
  std::vector<SubmapId> finished_submap_ids;
  std::set<NodeId> newly_finished_submap_node_ids;

  {
    absl::MutexLock locker(&mutex_);

    //得到该节点的数据.
    const auto& constant_data =
        data_.trajectory_nodes.at(node_id).constant_data;

    // 根据节点数据的时间获取最新的submap的id
    submap_ids = InitializeGlobalSubmapPoses(
        node_id.trajectory_id, constant_data->time, insertion_submaps);

    CHECK_EQ(submap_ids.size(), insertion_submaps.size());
    const SubmapId matching_id = submap_ids.front();

    //当前节点在局部地图中的位姿.
    const transform::Rigid2d local_pose_2d =
        transform::Project2D(constant_data->local_pose *
                             transform::Rigid3d::Rotation(
                                 constant_data->gravity_alignment.inverse()));

    //当前节点在全局地图中的位姿.
    const transform::Rigid2d global_pose_2d =
        optimization_problem_->submap_data().at(matching_id).global_pose *
        constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
        local_pose_2d;

    //把当前节点加入到pose-graph中.
    optimization_problem_->AddTrajectoryNode(
        matching_id.trajectory_id,
        optimization::NodeSpec2D{constant_data->time, local_pose_2d,
                                 global_pose_2d,
                                 constant_data->gravity_alignment});

    //加入当前节点和插入的子图之间的约束.插入的子图一般是两个
    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);
            
      // 加入到PoseGraph维护的容器中
      data_.submap_data.at(submap_id).node_ids.emplace(node_id);

       // 计算相对位姿
      const transform::Rigid2d constraint_transform =
          constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
          local_pose_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});
    }

    // TODO(gaschler): Consider not searching for constraints against
    // trajectories scheduled for deletion.
    // TODO(danielsievers): Add a member variable and avoid having to copy
    // them out here.
    // 得到所有完成了的submap.
    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);
      }
    }

    //如果最新的子图已经完成,把state设置为完成.
    if (newly_finished_submap)
    {
      const SubmapId newly_finished_submap_id = submap_ids.front();
      InternalSubmapData& finished_submap_data =
          data_.submap_data.at(newly_finished_submap_id);

      CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch);

      finished_submap_data.state = SubmapState::kFinished;

      newly_finished_submap_node_ids = finished_submap_data.node_ids;
    }
  }

新加入的节点与过去所有的子图进行匹配
新建立的子图，与过去所有的节点进行匹配
匹配的过程就是建立约束的过程，也就是回环检测的过程，因为Cartographer 采用的是图优化，有节点和约束的概念，，节点可以理解为激光点的位姿，约束就是激光数据与子图之间相对位姿
阅读代码发现Cartographer采用暴力搜索的方式，计算量巨大

  //对于所有的完成的子图都计算约束---实际上就是回环检测.
  //当前帧激光数据和以往所有的子图进行匹配.
  for (const auto& submap_id : finished_submap_ids)
  {
    ComputeConstraint(node_id, submap_id);
  }
 
  //如果新完成了一个submap,则需要计算当前pose-graph上的所有node,跟当前地图是否有匹配.
  //也可以认为是一个回环检测,不过是反过来的.计算过去的位姿跟当前子图的匹配.
  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.
    // 所有的老的node和当前子图的约束关系.
    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);
      }
    }
  }

  //约束计算完毕.
  constraint_builder_.NotifyEndOfNode();
  absl::MutexLock locker(&mutex_);

  //判断是否需要进行优化.
  ++num_nodes_since_last_loop_closure_;
  if (options_.optimize_every_n_nodes() > 0 &&
      num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes())
  {
    return WorkItem::Result::kRunOptimization;
  }
  return WorkItem::Result::kDoNotRunOptimization;
}

Cartographer的回环检测包括节点与子图，子图与节点的回环检测。回环检测完成后，函数最返回是否需要优化，如果需要优化，就开始后端优化

2.计算约束ComputeConstraint

进行约束计算,计算节点node_id和子图submap_id之间的约束.

在ComputeConstraintsForNode()中被调用.

void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
                                    const SubmapId& submap_id)
{
  bool maybe_add_local_constraint = false;
  bool maybe_add_global_constraint = false;
  const TrajectoryNode::Data* constant_data;
  const Submap2D* submap;

  {
    absl::MutexLock locker(&mutex_);
    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
    if (!data_.submap_data.at(submap_id).submap->insertion_finished())
    {
      // Uplink server only receives grids when they are finished, so skip
      // constraint search before that.
      return;
    }

    //获取该node和该submap最后一次具有约束的时间
    const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
    // 获取该node和该submap各自对应的trajectory上次连接的时间
    const common::Time last_connection_time =
        data_.trajectory_connectivity_state.LastConnectionTime(
            node_id.trajectory_id, submap_id.trajectory_id);

    //如果节点和子图属于同一条轨迹或者
    //最近有一个约束把该节点和子图的轨迹连接起来了,则有必要进行一个局部窗口搜索匹配.
    //因为他们很可能能连接上.
    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;
    }

    constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
    submap = static_cast<const Submap2D*>(
        data_.submap_data.at(submap_id).submap.get());
  }

  //如果是进行局部窗口搜索.--即有一个相对位姿.
  if (maybe_add_local_constraint)
  {
    //如果是同一条轨迹,则可以计算初始的相对位姿.
    const transform::Rigid2d initial_relative_pose =
        optimization_problem_->submap_data()
            .at(submap_id)
            .global_pose.inverse() *
        optimization_problem_->node_data().at(node_id).global_pose_2d;

    //计算约束.
    constraint_builder_.MaybeAddConstraint(
        submap_id, submap, node_id, constant_data, initial_relative_pose);
  }
  //如果是进行全局搜索.--即没有相对位姿.
  else if (maybe_add_global_constraint)
  {
    constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id,
                                                 constant_data);
  }
}

判断节点和子图是不是同一个轨迹，如果是同一个轨迹，就通过局部位姿，建立局部约束；如果不是同一个轨迹，就建立全局约束。
从本部分最后的代码看出，调用 constraint_builder_.MaybeAddConstraint建立局部约束；调用 constraint_builder_.MaybeAddGlobalConstraint建立局部约束。因此，constraint_builder_是具体的实现了约束的建立。

3.建立约束的具体实现ConstraintBuilder

该代码位于cartographer\mapping\internal\constraints、constraint_builder_2d

这部分代码根据建立局部约束还是全局约束，构造了不同的函数来调用同一个实现函数ComputeConstrain，这个函数最后调用了fast CSM ，这里就是实现了分支定界了的CSM

3.1局部约束

计算节点和子图之间的约束,本函数计算约束需要有一个初始位姿.
因此必须在同一个轨迹上才能满足.

void ConstraintBuilder2D::MaybeAddConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose)
{
  //超过搜索框的大小,则不进行搜索.
  if (initial_relative_pose.translation().norm() >
      options_.max_constraint_distance())
  {
    return;
  }

  if (!sampler_.Pulse()) return;

  absl::MutexLock locker(&mutex_);
  if (when_done_)
  {
    LOG(WARNING)
        << "MaybeAddConstraint was called while WhenDone was scheduled.";
  }

  constraints_.emplace_back();
  kQueueLengthMetric->Set(constraints_.size());

  auto* const constraint = &constraints_.back();

  //子图对应的fast scan-matcher.每一个submap都有一个fast scan-matcher.
  //为每一个子图，分配一个匹配器
  const auto* scan_matcher =
      DispatchScanMatcherConstruction(submap_id, submap->grid());

  //进行约束计算,类似于后端优化那一部分使用线性池，调用ComputeConstraint
  auto constraint_task = absl::make_unique<common::Task>();
  constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_)
  {
    ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
                      constant_data, initial_relative_pose, *scan_matcher,
                      constraint);
  });

  constraint_task->AddDependency(scan_matcher->creation_task_handle);

  //线程池的调度.
  auto constraint_task_handle =
      thread_pool_->Schedule(std::move(constraint_task));

  finish_node_task_->AddDependency(constraint_task_handle);
}

重要步骤：

每一个子图分配一个子图匹配器，调用了 DispatchScanMatcherConstruction（）
计算约束，调用了 ComputeConstraint

3.2建立全局约束

进行节点和子图之间的约束的计算,本次函数是全局匹配,不需要初始位姿.因此位姿设置为(0,0,0)
同时也不要求在同一个trajectory上.

void ConstraintBuilder2D::MaybeAddGlobalConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, const TrajectoryNode::Data* const constant_data)
{
  absl::MutexLock locker(&mutex_);
  if (when_done_)
  {
    LOG(WARNING)
        << "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
  }

  constraints_.emplace_back();
  kQueueLengthMetric->Set(constraints_.size());
  auto* const constraint = &constraints_.back();

  //得到子图对应的scan-matcher.
  const auto* scan_matcher =
      DispatchScanMatcherConstruction(submap_id, submap->grid());

  //进行约束计算.
  auto constraint_task = absl::make_unique<common::Task>();
  constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
    ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
                      constant_data, transform::Rigid2d::Identity(),
                      *scan_matcher, constraint);
  });

  //放入线程池中调用.
  constraint_task->AddDependency(scan_matcher->creation_task_handle);
  auto constraint_task_handle =
      thread_pool_->Schedule(std::move(constraint_task));
  finish_node_task_->AddDependency(constraint_task_handle);
}

步骤跟建立局部子图一样

3.3分配子图匹配器

为每一个子图submap_id分配一个scan-matcher.如果该子图已经有scan-matcher则直接返回,如果该子图没有scan-matcher.则为该子图分配一个。
涉及到金字塔的计算,因此需要通过线程的方法来进行计算.

const ConstraintBuilder2D::SubmapScanMatcher*
ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
                                                     const Grid2D* const grid)
{
  CHECK(grid);
  if (submap_scan_matchers_.count(submap_id) != 0)
  {
    return &submap_scan_matchers_.at(submap_id);
  }

  auto& submap_scan_matcher = submap_scan_matchers_[submap_id];

  submap_scan_matcher.grid = grid;
  //快速CSM
  auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options();

  auto scan_matcher_task = absl::make_unique<common::Task>();

  scan_matcher_task->SetWorkItem(
      [&submap_scan_matcher, &scan_matcher_options]() {
        submap_scan_matcher.fast_correlative_scan_matcher =
            absl::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
                *submap_scan_matcher.grid, scan_matcher_options);
      });
  submap_scan_matcher.creation_task_handle =
      thread_pool_->Schedule(std::move(scan_matcher_task));
  return &submap_scan_matchers_.at(submap_id);
}

3.4计算约束

计算节点和子图之间的位姿关系，用fast csm来进行匹配,然后用csm来进行优化.

void ConstraintBuilder2D::ComputeConstraint(
    const SubmapId& submap_id, const Submap2D* const submap,
    const NodeId& node_id, bool match_full_submap,
    const TrajectoryNode::Data* const constant_data,
    const transform::Rigid2d& initial_relative_pose,
    const SubmapScanMatcher& submap_scan_matcher,
    std::unique_ptr<ConstraintBuilder2D::Constraint>* constraint)
{
  CHECK(submap_scan_matcher.fast_correlative_scan_matcher);

  //匹配的初始位姿.
  const transform::Rigid2d initial_pose =
      ComputeSubmapPose(*submap) * initial_relative_pose;

  // The 'constraint_transform' (submap i <- node j) is computed from:
  // - a 'filtered_gravity_aligned_point_cloud' in node j,
  // - the initial guess 'initial_pose' for (map <- node j),
  // - the result 'pose_estimate' of Match() (map <- node j).
  // - the ComputeSubmapPose() (map <- submap i)
  // 节点j到子图i之间的约束关系:
  //
  float score = 0.;

  //初始位姿
  transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();

  // Compute 'pose_estimate' in three stages:
  // 1. Fast estimate using the fast correlative scan matcher.
  // 2. Prune if the score is too low.
  // 3. Refine.
  //make_full_submap就是无初始值，全局约束
  if (match_full_submap)
  {
    kGlobalConstraintsSearchedMetric->Increment();

    //进行无初始位姿fast csm匹配.
    //即全局匹配.
    if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
            constant_data->filtered_gravity_aligned_point_cloud,
            options_.global_localization_min_score(), &score, &pose_estimate))
    {
      CHECK_GT(score, options_.global_localization_min_score());
      CHECK_GE(node_id.trajectory_id, 0);
      CHECK_GE(submap_id.trajectory_id, 0);
      kGlobalConstraintsFoundMetric->Increment();
      kGlobalConstraintScoresMetric->Observe(score);
    }
    else
    {
      return;
    }
  }
  else
  {
    kConstraintsSearchedMetric->Increment();
    //进行有初始位姿的fast csm匹配.
    if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
            initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
            options_.min_score(), &score, &pose_estimate))
    {
      // We've reported a successful local match.
      CHECK_GT(score, options_.min_score());
      kConstraintsFoundMetric->Increment();
      kConstraintScoresMetric->Observe(score);
    }
    else
    {
      return;
    }
  }
  
  {
    absl::MutexLock locker(&mutex_);
    score_histogram_.Add(score);
  }

  // Use the CSM estimate as both the initial and previous pose. This has the
  // effect that, in the absence of better information, we prefer the original
  // CSM estimate.
  // 进行ceres-scan-match优化.
  ceres::Solver::Summary unused_summary;
  ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                            constant_data->filtered_gravity_aligned_point_cloud,
                            *submap_scan_matcher.grid, &pose_estimate,
                            &unused_summary);

  //计算得到的约束.
  const transform::Rigid2d constraint_transform =
      ComputeSubmapPose(*submap).inverse() * pose_estimate;

  //根据计算的位姿,重新设置约束的值.--可以认为增加了新的约束.
  constraint->reset(new Constraint{submap_id,
                                   node_id,
                                   {transform::Embed3D(constraint_transform),
                                    options_.loop_closure_translation_weight(),
                                    options_.loop_closure_rotation_weight()},
                                   Constraint::INTER_SUBMAP});

  if (options_.log_matches())
  {
    std::ostringstream info;
    info << "Node " << node_id << " with "
         << constant_data->filtered_gravity_aligned_point_cloud.size()
         << " points on submap " << submap_id << std::fixed;
    if (match_full_submap)
    {
      info << " matches";
    }
    else
    {
      const transform::Rigid2d difference =
          initial_pose.inverse() * pose_estimate;
      info << " differs by translation " << std::setprecision(2)
           << difference.translation().norm() << " rotation "
           << std::setprecision(3) << std::abs(difference.normalized_angle());
    }
    info << " with score " << std::setprecision(1) << 100. * score << "%.";
    LOG(INFO) << info.str();
  }
}

重要步骤：

全局匹配调用MatchFullSubmap，局部匹配调用Match函数；这两个函数均在fast_correlative_scan_matcher这个类里，而且调用的是同一个基函数
再进行ceres优化
计算约束（我的理解是进行一个位姿转换）
增加新约束，或者重设约束

小结

这部分的代码还是很清楚的，大家通读完代码之后就可以意识到Cartographer的函数调用特别多，所以其代码可读性很高。

这部分的代码终于把分支定界的CSM引出来了，下一篇文章就对fast csm进行解读。

你好哇zzz

关注

6
点赞
踩
25

收藏

觉得还不错? 一键收藏
0
评论
Cartographer源码阅读08 回环检测计算约束

本篇文章，将就如何实现回环检测和计算约束来进行深一步的讨论。代码位于cartographer\mapping\internal\2d\pose_graph_2d.cc
复制链接

扫一扫