cartographer探秘第四章之代码解析（二） --- 传感器数据处理过程
本次阅读的源码为 release-1.0 版本的代码
https://github.com/googlecartographer/cartographer_ros/tree/release-1.0
https://github.com/googlecartographer/cartographer/tree/release-1.0
也可以下载我上传的 全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,
https://download.csdn.net/download/tiancailx/11224156
第一篇文章简要分析了下 carto的代码是如何使用的，以及如何进行SLAM的。这篇文章将进雷达数据处理流程的分析。
在看 trajectory_builder_interface.h 之前需要了解几个数据类型。
cartographer/mapping/submaps.h
// An individual submap, which has a 'local_pose' in the local map frame, keeps
// track of how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no
// longer changes.
// 一个单独的子图，在local map frame中有一个'local_pose'，跟踪插入到其中的范围数据的数量，
// 并设置'finished_probability_grid'用于在地图不再更改时 loop closing。
class Submap {public:Submap(const transform::Rigid3d& local_submap_pose): local_pose_(local_submap_pose) {}// ...private:const transform::Rigid3d local_pose_;int num_range_data_ = 0;bool finished_ = false;
};cartographer/mapping/trajectory_node.h
struct TrajectoryNodePose {struct ConstantPoseData {common::Time time;transform::Rigid3d local_pose;};// The node pose in the global SLAM frame.transform::Rigid3d global_pose;common::optional<ConstantPoseData> constant_pose_data;
};struct TrajectoryNode {struct Data {common::Time time;// Transform to approximately gravity align the tracking frame as// determined by local SLAM.Eigen::Quaterniond gravity_alignment;// Used for loop closure in 2D: voxel filtered returns in the// 'gravity_alignment' frame.sensor::PointCloud filtered_gravity_aligned_point_cloud;// Used for loop closure in 3D.sensor::PointCloud high_resolution_point_cloud;sensor::PointCloud low_resolution_point_cloud;Eigen::VectorXf rotational_scan_matcher_histogram;// The node pose in the local SLAM frame.transform::Rigid3d local_pose;};common::Time time() const { return constant_data->time; }// This must be a shared_ptr. If the data is used for visualization while the// node is being trimmed, it must survive until all use finishes.std::shared_ptr<const Data> constant_data;// The node pose in the global SLAM frame.transform::Rigid3d global_pose;
};cartographer/mapping/trajectory_builder_interface.h
// This interface is used for both 2D and 3D SLAM. Implementations wire up a
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
// to detect loop closure, and a sparse pose graph optimization to compute
// optimized pose estimates.
class TrajectoryBuilderInterface {public:struct InsertionResult {NodeId node_id;std::shared_ptr<const TrajectoryNode::Data> constant_data;std::vector<std::shared_ptr<const Submap>> insertion_submaps;};// A callback which is called after local SLAM processes an accumulated// 'sensor::RangeData'. If the data was inserted into a submap, reports the// assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.using LocalSlamResultCallback =std::function<void(int /* trajectory ID */, common::Time,transform::Rigid3d /* local pose estimate */,sensor::RangeData /* in local frame */,std::unique_ptr<const InsertionResult>)>;SensorId
type 是传感器类型，id 是就是ros 的 话题，多个相同传感器时在 话题 名字后加 _1。
struct SensorId {enum class SensorType {RANGE = 0,IMU,ODOMETRY,FIXED_FRAME_POSE,LANDMARK,LOCAL_SLAM_RESULT};SensorType type;std::string id;
｝数据处理过程 AddSensorData()：
trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
cartographer/mapping/internal/collated_trajectory_builder.h
// cartographer/mapping/internal/collated_trajectory_builder.cc
class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface {
// ...
}CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::CollatorInterface* const sensor_collator, const int trajectory_id,const std::set<SensorId>& expected_sensor_ids,std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder): sensor_collator_(sensor_collator),trajectory_id_(trajectory_id),wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),last_logging_time_(std::chrono::steady_clock::now()) {std::unordered_set<std::string> expected_sensor_id_strings;for (const auto& sensor_id : expected_sensor_ids) {expected_sensor_id_strings.insert(sensor_id.id);}sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_id_strings,[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {HandleCollatedSensorData(sensor_id, std::move(data));});// cartographer/sensor/collator_interface.h
using Callback =std::function<void(const std::string&, std::unique_ptr<Data>)>;// Adds a trajectory to produce sorted sensor output for. Calls 'callback'// for each collated sensor data.virtual void AddTrajectory(int trajectory_id,const std::unordered_set<std::string>& expected_sensor_ids,const Callback& callback) = 0;// cartographer/sensor/internal/collator.ccvoid Collator::AddTrajectory(const int trajectory_id,const std::unordered_set<std::string>& expected_sensor_ids,const Callback& callback) {for (const auto& sensor_id : expected_sensor_ids) {const auto queue_key = QueueKey{trajectory_id, sensor_id};queue_.AddQueue(queue_key,[callback, sensor_id](std::unique_ptr<Data> data) {callback(sensor_id, std::move(data));});queue_keys_[trajectory_id].push_back(queue_key);}
}// cartographer/sensor/internal/ordered_multi_queue.cc
// 添加一个关键词是key的队列,并用比较函数Callback排序
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {CHECK_EQ(queues_.count(queue_key), 0);queues_[queue_key].callback = std::move(callback);
}sensor_collator->AddTrajectory() 的参数为：1 一个数据包就是一个trajectory_id，跑定位时会有２个id，多机器人SLAM会有很多个id， 2 expected_sensor_ids_strings 就是所有的 topic ，3 传入回调函数的函数名 - HandleCollatedSensorData() 。
调用了trajectory_builder_interface.h的AddSensorData()方法，这个方法是在collated_trajectory_builder.h 进行了具体的实现，它将 trajectory_id 和 传感器的数据 传入到队列中，cartographer/sensor/internal/ordered_multi_queue.h 这个文件声明了队列的定义. ordered_multi_queue.h 具体的说明请去 参考文献３进行查看，我就不多说了。
// cartographer/mapping/trajectory_builder_interface.hvirtual void AddSensorData(const std::string& sensor_id,const sensor::TimedPointCloudData& timed_point_cloud_data) = 0;virtual void AddSensorData(const std::string& sensor_id,const sensor::ImuData& imu_data) = 0;virtual void AddSensorData(const std::string& sensor_id,const sensor::OdometryData& odometry_data) = 0;virtual void AddSensorData(const std::string& sensor_id,const sensor::FixedFramePoseData& fixed_frame_pose) = 0;virtual void AddSensorData(const std::string& sensor_id,const sensor::LandmarkData& landmark_data) = 0;// cartographer/mapping/internal/collated_trajectory_builder.h
// 传感器的数据 data 是通过一个模板类 cartographer/sensor/internal/dispatchable.h 进行了多种数据类型的重载。void AddSensorData(const std::string& sensor_id,const sensor::TimedPointCloudData& timed_point_cloud_data) override {AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));}void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}// cartographer/sensor/internal/collator.cc
void Collator::AddSensorData(const int trajectory_id,std::unique_ptr<Data> data) {QueueKey queue_key{trajectory_id, data->GetSensorId()};queue_.Add(std::move(queue_key), std::move(data));
}// cartographer/sensor/internal/ordered_multi_queue.cc
// 根据key找到队列,并添加data元素, 调用一次Add()就要调用一次Dispatch()
void OrderedMultiQueue::Add(const QueueKey& queue_key,std::unique_ptr<Data> data) {auto it = queues_.find(queue_key);if (it == queues_.end()) {LOG_EVERY_N(WARNING, 1000)<< "Ignored data for queue: '" << queue_key << "'";return;}it->second.queue.Push(std::move(data));Dispatch();
}/*
OrderedMultiQueue，用于管理多个有序的传感器数据，
是有序的多队列类,每个队列有一个key,并且有一个自定义排序函数
queues_的形式为：
key1:Queue
key2：Queue
key3：QueueQueue的形式为struct Queue {common::BlockingQueue<std::unique_ptr<Data>> queue;Callback callback;bool finished = false;};OrderedMultiQueue的数据成员有
1,common_start_time_per_trajectory_:轨迹id及对应创建轨迹时间
2,last_dispatched_time_
3,std::map<QueueKey, Queue> queues_;按照key排序的map
4,QueueKey blocker_;---------------------
作者：slamcode
来源：CSDN
原文：https://blog.csdn.net/learnmoreonce/article/details/76218106
版权声明：本文为博主原创文章，转载请附上博文链接！
*/也就是说，队列的个数为：每个轨迹所订阅的话题数。如下，２个轨迹，轨迹１订阅２个话题，轨迹２订阅１个话题，那我的队列的map就只有３个key，也就是３个队列。
// These are keys are chosen so that they sort first, second, third.const QueueKey kFirst{1, "a"};const QueueKey kSecond{1, "b"};const QueueKey kThird{2, "b"};总结：每次数据到来，根据，调用trajectory_builder_interface.h的AddSensorData()，再调用ordered_multi_queue.cc的Add()方法，再调用OrderedMultiQueue::Dispatch()方法，Dispatch()方法使用 回调函数CollatedTrajectoryBuilder::HandleCollatedSensorData() 处理 根据数据的时间已经排好序的数据队列。
// cartographer/mapping/internal/collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::HandleCollatedSensorData(const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {auto it = rate_timers_.find(sensor_id);if (it == rate_timers_.end()) {it = rate_timers_.emplace(std::piecewise_construct, std::forward_as_tuple(sensor_id),std::forward_as_tuple(common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))).first;}it->second.Pulse(data->GetTime());if (std::chrono::steady_clock::now() - last_logging_time_ >common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {for (const auto& pair : rate_timers_) {LOG(INFO) << pair.first << " rate: " << pair.second.DebugString();}last_logging_time_ = std::chrono::steady_clock::now();}data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}// 也就是跑carto时候的消息：
// [ INFO]: collated_trajectory_builder.cc:72] imu rate: 10.00 Hz 1.00e-01 s +/- 4.35e-05 s (pulsed at 100.44% real time)
// [ INFO]: collated_trajectory_builder.cc:72] scan rate: 19.83 Hz 5.04e-02 s +/- 4.27e-05 s (pulsed at 99.82% real time)// cartographer/sensor/internal/dispatchable.hvoid AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *const trajectory_builder) override {trajectory_builder->AddSensorData(sensor_id_, data_);}之后在通过CollatedTrajectoryBuilder::HandleCollatedSensorData() 方法将这个 已经排好序的数据队列集合 传回 Local SLAM. 那这是怎么传的呢？又看了好久才看懂这个转换。
首先，先进入到 map_builder.cc 中。这是整体SLAM的处理过程。
cartographer/mapping/map_builder.cc
// cartographer/mapping/map_builder.cc
int MapBuilder::AddTrajectoryBuilder(const std::set<SensorId>& expected_sensor_ids,const proto::TrajectoryBuilderOptions& trajectory_options,LocalSlamResultCallback local_slam_result_callback) {const int trajectory_id = trajectory_builders_.size();if (options_.use_trajectory_builder_3d()) {
...
} else {std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;if (trajectory_options.has_trajectory_builder_2d_options()) {local_trajectory_builder = common::make_unique<LocalTrajectoryBuilder2D>(trajectory_options.trajectory_builder_2d_options(),SelectRangeSensorIds(expected_sensor_ids));}DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));trajectory_builders_.push_back(// 看这里common::make_unique<CollatedTrajectoryBuilder>(sensor_collator_.get(), trajectory_id, expected_sensor_ids,CreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder), trajectory_id,static_cast<PoseGraph2D*>(pose_graph_.get()),local_slam_result_callback)));
...}AddTrajectoryBuilder() 方法就是一个整体SLAM的初始化方法。
首先我们这里只看2d SLAM 的部分，也就是在上述代码中标记的 看这里 。 这块代码执行了如下功能，首先，将CollatedTrajectoryBuilder() 进行初始化，并且传入 sensor_collator_.get(), trajectory_id 和 expected_sensor_ids 参数。expected_sensor_ids是调用AddTrajectoryBuilder()时的参数输入，代表着所有传感器话题的集合。同时，也传入了一个 CreateGlobalTrajectoryBuilder2D() 的函数名，这是GlobalTrajectoryBuilder类的构造函数，直接传入函数名就是传入函数的地址，而正好CollatedTrajectoryBuilder()构造函数的第四个参数是个指针 std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder . 之后又使用wrapped_trajectory_builder进行右值转换后对 wrapped_trajectory_builder_进行赋值。
之后使用HandleCollatedSensorData()将这个指针指向的数据传入到 data的AddToTrajectoryBuilder() 方法中，也就是将GlobalTrajectoryBuilder类的函数指针（？？？对不对，是传入地址指针还是传入什么）传入进去。
由于GlobalTrajectoryBuilder类是继承于mapping::TrajectoryBuilderInterface的，所以指针的类型是 std::unique_ptr<TrajectoryBuilderInterface> 。
之后在dispatchable.h 文件中的 AddToTrajectoryBuilder() 方法 调用 GlobalTrajectoryBuilder类下的AddSensorData().
// cartographer/mapping/internal/collated_trajectory_builder.cc
void CollatedTrajectoryBuilder::HandleCollatedSensorData() {// ...data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}// cartographer/sensor/internal/dispatchable.hvoid AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *const trajectory_builder) override {trajectory_builder->AddSensorData(sensor_id_, data_);}GlobalTrajectoryBuilder类下的AddSensorData() 将进行 local slam 的扫描匹配，下篇文章再讲。
references
1. https://blog.csdn.net/liuxiaofei823/article/details/70207814
2. https://blog.csdn.net/roadseek_zw/article/details/66968762
3. https://blog.csdn.net/u012209790/article/details/82735923
4. https://blog.csdn.net/learnmoreonce/article/details/76218106
5. https://blog.csdn.net/learnmoreonce/article/details/73718535
cartographer探秘第四章之代码解析（二） --- 传感器数据处理过程相关推荐
- 学习笔记-第十四章 恶意代码分析实战
第十四章 恶意代码的网络特征 1.网络应对措施. 网络行为的基本属性包括IP地址,TCP端口,以及流量内容等,网络和安全 设备可以利用它们,来提供网络应对措施.根据IP地址和端口,防火墙和路由器可以限 ...
- 第四章 PX4-Pixhawk-MPU6000传感器驱动解析
第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解析步骤同样可以对L3GD20(3轴陀螺仪).HMC58 ...
- 第四章MPU6000传感器驱动解析
版权声明:本文为博主原创文章,未经博主允许不得转载. 第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解 ...
- Kali Linux 网络扫描秘籍 第四章 指纹识别（二）
第四章 指纹识别(二) 作者:Justin Hutchens 译者:飞龙 协议:CC BY-NC-SA 4.0 4.6 Nmap 服务识别 虽然特征抓取是非常有利的信息来源,服务特征中的版本发现越来越 ...
- Foundations of Machine Learning 2nd——第四章Model Selection（二）
Foundations of Machine Learning 2nd--第四章Model Selection(二) 交叉验证 Cross Validation(CV) 交叉验证的步骤 交叉验证有效性 ...
- CodeCombat代码全记录（Python学习利器）--安息之云山峰（第四章）代码1
我们已经有了前三章的基础了,到了第四章,你会发现提示少之又少了.这时,我们大都要靠自己了!!!加油!!如果你没有思路,请仔细看下装备的说明,毕竟到了这关,你需要购买好多其他的新装备,而新装备中存在了新 ...
- Java 北大青鸟 第一学期 第四章 选择结构（二） 上级练习+课后作业
第一学期 第四章 选择结构二 示例1 实现购物菜单 实现换购的功能 本章练习一 计算器 下载地址 1. 选择结构 基本if选择结构 if(条件){条件成立时执行的代码} if-else选择结构 if( ...
- ANSYS结构非线性分析指南连载四--第四章 材料非线性分析 (二)
http://blog.sina.com.cn/s/blog_531237a90101ioxg.html 原地址 4.3 超弹性分析 4.3.1 超弹理论 4.3.1.1 超弹的定义 一般工程材料(例 ...
- Paxos算法之旅（四）zookeeper代码解析--转载
ZooKeeper是近期比较热门的一个类Paxos实现.也是一个逐渐得到广泛应用的开源的分布式锁服务实现.被认为是Chubby的开源版,虽然具体实现有很多差异.ZooKeeper概要的介绍可以看官方文 ...
- 一个基于JRTPLIB的轻量级RTSP客户端(myRTSPClient)——收流篇：（四）example代码解析...
--------------------更新2018.08.20------------------- 添加http_tunnel_example.cpp作为RtspOverHttp示例程序. --- ...
最新文章
- numpy维度交换_15年！NumPy论文终出炉，还登上了Nature
- mysql dba系统学习（3）mysql的启动停止
- Keras:基于Theano和TensorFlow的深度学习库
- use SQVI to display table join
- php date 有warning,PHP Warning: strtotime()错误解决办法
- python编程之路——类和对象
- 1.9_heap_topK_topK问题
- 图解25匹马的选马问题
- python不定长参数_Python可变长参数
- 对于拖延症的最好方法
- ospf路由 华3_华三路由器命令信息
- 贴吧怎么发帖，发防删图出现审核怎么办？
- Dell EMC VxRail
- 黄大成：中国浙江的“盖茨神话”
- MineCraft Spigot简单开服教程
- vrchat新手教程_VRChat入门指南| 最新电脑资讯
- C和指针 第9章 字符串、字符和字节 9.14 编程练习
- altera系列fifo和ram
- 无线网dns的服务器地址是多少,宽带通dns的服务器地址是多少
- 对话京东安全首席架构师：电商平台构建安全防护体系关键点