1.点云同步处理的类  RangeDataCollator

    class RangeDataCollator {
public:
explicit RangeDataCollator(
const std::vector<std::string>& expected_range_sensor_ids)
: expected_sensor_ids_(expected_range_sensor_ids.begin(),
expected_range_sensor_ids.end()) {} sensor::TimedPointCloudOriginData AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data); private:
sensor::TimedPointCloudOriginData CropAndMerge(); const std::set<std::string> expected_sensor_ids_;
// Store at most one message for each sensor.
std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
common::Time current_start_ = common::Time::min();
common::Time current_end_ = common::Time::min();
};

其中有四个成员函数:

生成点云的所有传感器的 sensor_ids

      const std::set<std::string> expected_sensor_ids_;

修剪合并点云时的所有点云  sensor_id  和与之对应的数据

     std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;

时间撮  记录点云

      common::Time current_start_ = common::Time::min();
common::Time current_end_ = common::Time::min();

2.AddRangeData

    sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data)

1.首先检测该点云数据 sensor_id 是否在期望的sensor_ids里面,否则程序停止

  CHECK_NE(expected_sensor_ids_.count(sensor_id), );

2.当该sensor_id 已经在 id_to_pending_data_ 中

当我们有两个相同传感器的消息时,移除两个中旧的数据,但不发送电流

1>跟新current_start,current_end 时间

    current_start_ = current_end_;
current_end_ = id_to_pending_data_.at(sensor_id).time;

2>修剪合并在  id_to_pending_data_ 中的数据  CropAndMerge

    auto result = CropAndMerge();

3>将当前传感器数据放入 id_to_pending_data_ 中,并且返回result

        id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
return result;

3.当sensor_id 不在 id_to_pending_data_ 中时

1>首先当前数据加到 id_to_pending_data_ 中,

  id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);

2>使 id_to_pending_data_ 中 包含期望的所有数据

      if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
return {};
}

3>跟新时间

      current_start_ = current_end_;
// We have messages from all sensors, move forward to oldest.
common::Time oldest_timestamp = common::Time::max();
for (const auto& pair : id_to_pending_data_) {
oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
}
current_end_ = oldest_timestamp;

4>修剪合并  CropAndMerge
3.CropAndMerge

sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge()

1.定义返回对象

  sensor::TimedPointCloudOriginData result{current_end_, {}, {}};

2.遍历 id_to_pending_data_ 中所有传感器的数据,其对于传感器数据处理一样

      for (auto it = id_to_pending_data_.begin();
it != id_to_pending_data_.end();) {
sensor::TimedPointCloudData& data = it->second;
sensor::TimedPointCloud& ranges = it->second.ranges;

2>将在[current_start_,current_end_]区间内的传感器数据  下表确定出来

        auto overlap_begin = ranges.begin();
while (overlap_begin < ranges.end() &&
data.time + common::FromSeconds((*overlap_begin)[]) <
current_start_) {
++overlap_begin;
}
auto overlap_end = overlap_begin;
while (overlap_end < ranges.end() &&
data.time + common::FromSeconds((*overlap_end)[]) <= current_end_) {
++overlap_end;
}

3>复制重叠范围。

        if (overlap_begin < overlap_end) {
std::size_t origin_index = result.origins.size();
result.origins.push_back(data.origin);
double time_correction = common::ToSeconds(data.time - current_end_);
for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
++overlap_it) {
sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it,
origin_index};
// current_end_ + point_time[3]_after == in_timestamp +
// point_time[3]_before
point.point_time[] += time_correction;
result.ranges.push_back(point);
}
}

涉及到:   处理了前一时刻的点云,然后将时间跟新

4>将缓冲点丢弃,直到overlap_end。

        if (overlap_end == ranges.end()) {
it = id_to_pending_data_.erase(it);
} else if (overlap_end == ranges.begin()) {
++it;
} else {
data = sensor::TimedPointCloudData{
data.time, data.origin,
sensor::TimedPointCloud(overlap_end, ranges.end())};
++it;
}

3.按照时间排序,并且返回结果

      std::sort(result.ranges.begin(), result.ranges.end(),
[](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
return a.point_time[] < b.point_time[];
});
return result;

感觉cartographer处理点云数据有一帧延时

原文地址:https://blog.csdn.net/xiaoma_bk/article/details/85261422

cartographer 点云同步处理的更多相关文章

  1. 配置mac百度云同步盘

    1. 选择同步盘在电脑中的位置,该文件夹中的内容与云端保持一致.默认位置/Users/LemonVerbena/百度云同步盘.电脑同步盘的作用与百度云网盘的主页一样,下面可以包括多个同步文件夹. 2. ...

  2. 打造自己的MyLifeOrganized 2(MLO2)云同步

    0x01 官方云同步,付费也很卡 MyLifeOrganized(MLO)是Windows平台下强大的GTD软件,PC版本和Android版本需要分别购买授权,云同步还要再买包月或包年服务真不便宜,关 ...

  3. 黄聪:VPS实现自动定时备份网站数据以及Mysql数据库到百度云同步盘

    建站多了,备份成了头疼的问题,因为你不知道你的VPS什么时候会宕机或者服务商跑路,一旦网站数据丢失,那么相当于前功尽弃了,所以自己研究出了一套自动备份的方法. 需要的东西: 1.一个VPS(虚拟空间没 ...

  4. 小白也能用Git管理团队项目了:百度云同步+Git Extensions+Git Source Control Provider

    百度云同步 百度云同步,会将本地的某个文件目录和云端进行同步.如果在本地将这个同步的目录设置为Git的中心服务器,那么本地push到中心服务器的内容也会被同步到云端.其他开发者只要也进行相同的设置,就 ...

  5. 使用百度云同步盘和Git Extensions进行代码托管

    一.软件安装 百度云同步盘和Git Extensions的安装和配置略过. 二.建立Git代码库,以[GHO2VMDK转换工具]项目为例 1. 建立本地代码库 进入[GHO2VMDK转换工具]项目文件 ...

  6. leanote使用本地账户+坚果云同步

    1. 换机器后笔记无法显示 这是因为新建账户与原账户userid不一致. 正确的同步方式为: 下载leanote并解压,不运行,不新建账户 从坚果云同步leanote数据 创建leanote的数据目录 ...

  7. 亿级日PV的魅族云同步的核心协议与架构实践(转)

    云同步的业务场景 这是魅族云同步的演进,第一张是M8.M9,然后到后面的是MX系统,M9再往后发展,我们的界面可以看到基本上是没有什么变化的,但本质发生了很大的变化,我们经过了一些协议优化,发展到今天 ...

  8. 亿级日PV的魅族云同步的核心协议与架构实践

    声明:本文根据msup和魅族联合举办的<第三期魅族技术开放日-架构设计与优化>录音整理原创首发,转载或节选内容前需获授权. 嘉宾:沈辉煌,魅族高级架构师,魅族云同步负责人.2010年加入魅 ...

  9. 使用BestSync同步软件与坚果云同步

    坚果云的免费用户可以享受每个月的1G上传与3G下载流量,同时号称是国内唯一支持WebDAV的云.我的工作备份的文档不多,正好手头有BestSync同步软件可以用.决定试试BestSync的与WebDA ...

随机推荐

  1. python3 json模块

    import json '''把python对象转化为json串(字符串), ensure_ascii处理中文乱码'''dic = {"复联4": "好看吗", ...

  2. multiset的erase()操作中出现跳过元素的问题

    昨天,我写了一个multiset去重,让tt指向it的后面第一个元素,若重复则删除这2个元素,并令it=tt,it++:来使it指向tt的下一个元素(我想指向原it的后面第2个元素,并认为tt的下一个 ...

  3. 乡下人设计模式——SOLID之六大原则

    S(Single Responsibility Principle):单一责任原则 O(Open Closed Principle):开放封闭原则 L(Liskov Substitution Prin ...

  4. Leetcode 27. Remove Element(too easy)

    Given an array and a value, remove all instances of that value in-place and return the new length. D ...

  5. Oracle的表被锁后的恢复

    运行下列SQL,找出数据库的serial#,执行结果如下图所示 SELECT T2.USERNAME, T2.SID, T2.SERIAL#, T2.LOGON_TIME   FROM V$LOCKE ...

  6. 控制结构(11): Continuation passing style(CPS)

    // 上一篇:控制结构(10)指令序列(opcode) [注释]: 这个笔记系列需要告一个段落了,收尾部分整理下几个时髦(The New Old Things)结构. 后面打算开一个算法方面的,重新学 ...

  7. mybatis 使用auto mapping原理实现表间关联

    Auto mapping的示例 数据库中有一个person表,结构如下: mysql> desc person; +-------+-------------+------+-----+---- ...

  8. linux下安装PHP扩展memcache

    公司的服务器 CentOS 7.5,PHP 5.6 下面都是最新的版本(支持到PHP5.6) 如需php7  下支持memcache扩展,请移步   https://www.cnblogs.com/h ...

  9. A Java Runtime Environment (JRE) or Java Development Kit (JDK) must be available in order to run Eclipse.

    cp -r /home/cuthead/android-studio/jre /home/cuthead/adt-bundle-linux-x86_64-20131030/eclipse/jre 使用 ...

  10. POJChallengeRound2 Tree 【数学期望】

    题目分析: 我们令$G(x)$表示前$x$个点的平均深度,$F(x)$表示第$x$个点的期望深度. 有$F(x) = G(x-1)+1$,$G(x) = G(x-1)+\frac{1}{x}$ 所以答 ...