Cartographer源码阅读---番外篇: Submap封装与维护

这篇具有很好参考价值的文章主要介绍了Cartographer源码阅读---番外篇: Submap封装与维护。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

Cartographer中Submap(子图)没有被直接的调用进行维护, 而是针对2D和3D场景分别派生出子类Submap2D和Submap3D, 进行调用. 以2D为例, 为了方便维护, 又把Submap2D封装成了ActiveSubmaps2D进行维护, 其维护方式类似与滑窗, 也是只维护最近的一些数据.

1. Submap类

/**
 * @brief 独立的子地图, 3个功能
 * 
 * 保存在local坐标系下的子图的坐标
 * 记录插入到子图中雷达数据的个数
 * 标记这个子图是否是完成状态
 */
class Submap {
 public:

  // 构造函数, 将传入的local_submap_pose作为子图的坐标原点
  Submap(const transform::Rigid3d& local_submap_pose)
      : local_pose_(local_submap_pose) {}
  virtual ~Submap() {}

  virtual proto::Submap ToProto(bool include_grid_data) const = 0;
  virtual void UpdateFromProto(const proto::Submap& proto) = 0;

  // Fills data into the 'response'.
  virtual void ToResponseProto(
      const transform::Rigid3d& global_submap_pose,
      proto::SubmapQuery::Response* response) const = 0;

  // Pose of this submap in the local map frame.
  // 在local坐标系的子图的坐标
  transform::Rigid3d local_pose() const { return local_pose_; }

  // Number of RangeData inserted.
  // 插入到子图中雷达数据的个数
  int num_range_data() const { return num_range_data_; }
  void set_num_range_data(const int num_range_data) {
    num_range_data_ = num_range_data;
  }

  bool insertion_finished() const { return insertion_finished_; }
  // 将子图标记为完成状态
  void set_insertion_finished(bool insertion_finished) {
    insertion_finished_ = insertion_finished;
  }

 private:
  const transform::Rigid3d local_pose_; // 子图原点在local坐标系下的坐标
  int num_range_data_ = 0;
  bool insertion_finished_ = false;
};

从私有变量可以看到, Submap维护了三个东西

  1. 自己的坐标, 这个坐标是在local坐标系下的坐标
  2. 本子图中节点的个数, 也就是有几帧雷达数据
  3. 是不是个结束插入的子图, 节点数大于一定值后就不能在插入雷达数据了

Submap的坐标系在构造的时候就得到了, 是子图第一帧激光雷达的坐标系, 分析如下
Submap的第一次构造在

  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);

中, 调用的是active_submap的InsertRangeData这个函数.
在submap_2d.cc中看 ActiveSubmaps2D::InsertRangeData这个函数,

// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  // 如果第二个子图插入节点的数据等于num_range_data时,就新建个子图
  // 因为这时第一个子图应该已经处于完成状态了
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
  // 将一帧雷达数据同时写入两个子图中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(
        range_data,
        range_data_inserter_.get());  // 是submap类的, 不是ActiveSubmaps2D的InsertRangeData
  }
  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { //一个子图有180个扫描
    submaps_.front()->Finish();
  }
  return submaps();
}

其中新插入子图是调用了AddSubmap(range_data.origin.head<2>()),

// 新增一个子图,根据子图个数判断是否删掉第一个子图
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
  // 调用AddSubmap时第一个子图一定是完成状态,所以子图数为2时就可以删掉第一个子图了
  if (submaps_.size() >= 2) {
    // This will crop the finished Submap before inserting a new Submap to
    // reduce peak memory usage a bit.
    CHECK(submaps_.front()->insertion_finished());
    // 删掉第一个子图的指针
    submaps_.erase(submaps_.begin());
  }
  // 新建一个子图, 并保存指向新子图的智能指针
  submaps_.push_back(absl::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release())),
      &conversion_tables_));
}

也就是把range_data.origin.head<2>()放进来作为submap的原点, 再看看range_data

/**
 * @brief local_slam_data中存储所有雷达点云的数据结构
 * 
 * @param origin  点云的原点在local坐标系下的坐标
 * @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit
 * @param misses  是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点
 */
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses; // local坐标系下的坐标
};

origin就是这个点云在local坐标系下的坐标.
所以说submap的坐标就是submap第一帧点云点在local坐标系下的坐标.

2. submap2D类

3. ActiveSubmap2D类

ActiveSubmaps2D类中的submaps_列表实际最多只两个submap,一个认为是old_map,另一个认为是new_map,类似于滑窗操作。当new_map插入激光scan的个数达到阈值时,则会将old_map进行结束,并且不再增加新的scan。同时将old_map进行删除,将new_map作为oldmap,然后重新初始化一个新的submap作为newmap。代码很简单
Cartographer源码阅读---番外篇: Submap封装与维护

// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
    const sensor::RangeData& range_data) {
  // 如果第二个子图插入节点的数据等于num_range_data时,就新建个子图
  // 因为这时第一个子图应该已经处于完成状态了
  if (submaps_.empty() ||
      submaps_.back()->num_range_data() == options_.num_range_data()) {
    AddSubmap(range_data.origin.head<2>());
  }
  // 将一帧雷达数据同时写入两个子图中
  for (auto& submap : submaps_) {
    submap->InsertRangeData(
        range_data,
        range_data_inserter_.get());  // 是submap类的, 不是ActiveSubmaps2D的InsertRangeData
  }
  // 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_data
  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) { //一个子图有180个扫描
    submaps_.front()->Finish();
  }
  return submaps();
}
......
// 新增一个子图,根据子图个数判断是否删掉第一个子图
void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
  // 调用AddSubmap时第一个子图一定是完成状态,所以子图数为2时就可以删掉第一个子图了
  if (submaps_.size() >= 2) {
    // This will crop the finished Submap before inserting a new Submap to
    // reduce peak memory usage a bit.
    CHECK(submaps_.front()->insertion_finished());
    // 删掉第一个子图的指针
    submaps_.erase(submaps_.begin());
  }
  // 新建一个子图, 并保存指向新子图的智能指针
  submaps_.push_back(absl::make_unique<Submap2D>(
      origin,
      std::unique_ptr<Grid2D>(
          static_cast<Grid2D*>(CreateGrid(origin).release())),
      &conversion_tables_));
}

文章来源地址https://www.toymoban.com/news/detail-451118.html

到了这里,关于Cartographer源码阅读---番外篇: Submap封装与维护的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • Unity 事件番外篇:UnityEvent

    前置知识: C#委托 C#事件 简要概括:使用 UnityEvent 可以在编辑器的 Inspector 面板中为事件绑定事件触发函数。 下文将会着重介绍一些细节。 之前在介绍委托的时候有提到 UntiyAction,它是 Unity 对 C# Action 委托的一个封装。而本文将要介绍的 UnityEvent,则是对 C# 事件的一个封装

    2024年02月09日
    浏览(34)
  • C++番外篇之动态爱心代码

    前言:今天我们给大家介绍一个有趣的代码,那就是爱心代码,前提是这段代码要先下载一个东西,就是有关C++头文件的,这段代码各位看看就好,当个乐子,因为涉及的代码知识很多。如果大家有兴趣研究的,可以把整段代码看一看。 下面直接先展现代码了: 这里是运行

    2024年02月05日
    浏览(39)
  • 番外篇 萌新版开发交付一条龙(☆▽☆)

    学习了一段时间的django和vue,对于前后端开发有了一个初步的了解,这里记录一下编写的流程和思路,主要是为了后面如果遗忘从哪里开始操作做一个起步引导作用 参考下前面django的文档https://moziang.blog.csdn.net/article/details/130720709 1、安装django环境 目录结构 2、项目添加应用模

    2024年02月21日
    浏览(37)
  • 【flink番外篇】12、ParameterTool使用示例

    一、Flink 专栏 Flink 专栏系统介绍某一知识点,并辅以具体的示例进行说明。 1、Flink 部署系列 本部分介绍Flink的部署、配置相关基础内容。 2、Flink基础系列 本部分介绍Flink 的基础部分,比如术语、架构、编程模型、编程指南、基本的datastream api用法、四大基石等内容。 3、

    2024年01月18日
    浏览(56)
  • 【flink番外篇】16、DataStream 和 Table 相互转换示例

    一、Flink 专栏 Flink 专栏系统介绍某一知识点,并辅以具体的示例进行说明。 1、Flink 部署系列 本部分介绍Flink的部署、配置相关基础内容。 2、Flink基础系列 本部分介绍Flink 的基础部分,比如术语、架构、编程模型、编程指南、基本的datastream api用法、四大基石等内容。 3、

    2024年01月17日
    浏览(61)
  • 【Unity】Avatar与AvatarMask系统介绍(TPS.番外篇)

    这次也是拖了蛮久,一个是在修动画,一个是别的游戏确实比较能吸引住人。 在主要系列进行前,要先为接下来要讲的动画做一些基础知识的补充,这期是Avatar,即替身系统,以及AvatarMask的讲解。 对于动画,我的了解比较基础,大家可以去看这位的系列《动画入门》。 在这

    2024年02月09日
    浏览(35)
  • 【flink番外篇】13、Broadcast State 模式示例(完整版)

    一、Flink 专栏 Flink 专栏系统介绍某一知识点,并辅以具体的示例进行说明。 1、Flink 部署系列 本部分介绍Flink的部署、配置相关基础内容。 2、Flink基础系列 本部分介绍Flink 的基础部分,比如术语、架构、编程模型、编程指南、基本的datastream api用法、四大基石等内容。 3、

    2024年01月17日
    浏览(53)
  • 番外篇Diffusion&Stable Diffusion扩散模型与稳定扩散模型

    本篇文章为阅读笔记,,主要内容围绕扩散模型和稳定扩散模型展开,介绍了kl loss、vae模型的损失函数以及变分下限作为扩展部分。扩散模型是一种生成模型,定义了一个逐渐扩散的马尔科夫链,逐渐项数据添加噪声,然后学习逆扩散过程,从噪声中构建所需的数据样本。稳

    2024年02月03日
    浏览(51)
  • 算法通关村番外篇-LeetCode编程从0到1系列一

    大家好我是苏麟 , 今天开始带来LeetCode编程从0到1系列 . 编程基础 0 到 1 , 50 题掌握基础编程能力 描述 : 给你两个字符串 word1 和 word2 。请你从 word1 开始,通过交替添加字母来合并字符串。如果一个字符串比另一个字符串长,就将多出来的字母追加到合并后字符串的末尾。 返

    2024年01月21日
    浏览(32)
  • 算法通关村番外篇-LeetCode编程从0到1系列二

    大家好我是苏麟 , 今天来说LeetCode编程从0到1系列二 . 内置函数 描述 : 给你一个字符串  s ,由若干单词组成,单词前后用一些空格字符隔开。返回字符串中  最后一个  单词的长度。 单词  是指仅由字母组成、不包含任何空格字符的最大子字符串。 题目 : LeetCode 58. 最后一

    2024年01月21日
    浏览(34)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包