从代码理解 cartographer 2
从上一篇开始, 我很着急地去希望看到雷达信息是如订阅的. 只要有了订阅的开始, 那么数据就会被回调到回调函数,才知道那些VoxelFilter所处理的数据, 在某个特定的情况下,实际上是怎么做的.
我首先是按照着上面的想法,开始看回调函数. 当到了某一定程度之后. 会产生迷茫, 这到底调用的是哪个对象的哪个函数.
这样主要是因为谷歌为了同时适用3D和2D情况. 用了虚类设计了许多接口, 2D和3D的按着这些借口来实现.
并且ROS定义的数据结构和cartographer使用的,也有些不同, 他们需要进行转换. 如雷达信息就需要转换为点云.
所以我们先通过一定的代码, 来了解他们的组织.
通过这些阅读, 使我懂得了他们的组织, 理解了是这样的一个跳转.
之前知乎的源码解读系列也有一个LocalSlam和各个组织的关系. 我也是由于看到一半,没懂得作者所说, 一知半解地知道全部,不如我从新读一次,了解完LocalSLAM.
所以读得时候, 有许多东西可能参考了 知乎的源码解读系列.
我在读他人得解读的时候, 我发现一个问题, 前辈们都读得比较早. 跟我现在读得版本不一样.
我先说明我自己的版本是 1.0.0. 时间是2019-09-04.
然后是我所总结出来的这几个对象的包含关系是:
Version:1.0 StartHTML:0000000107 EndHTML:0000046460 StartFragment:0000030811 EndFragment:0000046446
node:
sensor_samplers_:
extrapolators_:
map_builder_bridge_:
map_builder_:
pose_graph_:
trajectory_builders_: CollatedTrajectoryBuilder, CreateGlobalTrajectoryBuilder2D(),
wrapped_trajectory_builder_: GlobalTrajectoryBuilder
pose_graph_:PoseGraph2D
local_trajectory_builder_: LocalTrajectoryBuilder2D
range_data_collator_：RangeDataCollator
real_time_correlative_scan_matcher_: RealTimeCorrelativeScanMatcher2D
ceres_scan_matcher_: CeresScanMatcher2D
extrapolator_:
active_submaps_:
sensor_collator_: (和map_builder_是一致的)
HandleCollatedSensorData:(函数)
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get())
sensor_collator_: Collator 或 TrajectoryCollator
OrderedMultiQueue
sensor_bridges_:SensorBridge
tf_bridge_
trajectory_builder_: (初始化是 map_builder_->GetTrajectoryBuilder(trajectory_id)), CollatedTrajectoryBuilder
wrapped_trajectory_builder_: GlobalTrajectoryBuilder
pose_graph_:PoseGraph2D
local_trajectory_builder_: LocalTrajectoryBuilder2D
range_data_collator_：RangeDataCollator
real_time_correlative_scan_matcher_: RealTimeCorrelativeScanMatcher2D
ceres_scan_matcher_: CeresScanMatcher2D
extrapolator_:
active_submaps_:
pose_graph_: PoseGraph2D, 都是上面传进来的指针
sensor_collator_: Collator 或 TrajectoryCollator
queue_: OrderedMultiQueue,IMU数据放到了这儿, odometry同样也是, laser的数据也是
这就是本篇所得结论的关系拉.
其实是能够从类的头文件里面得出来的.可能有同学能够用工具,一下就生成了. 我是不知道这样的工具(在评论下可以直接告诉我啊).
但是我依然觉得我的方法没错多少. 因为这儿的每一个类, 都是跟构图的主脉络有关的.
如果工具生成, 可能有许多不相关得东西要进行剔除.
我先解释下这个图.
首先是 node, 这是一个Class, 在第一篇提到过,它进行了主题得订阅.
现在在node的下面, 有三个主要的对象, 分别是 sensor_samplers_, extrapolators_ 和 map_builder_bridge_
然后,按同样的缩进包含的法则
map_builder_bridge_ 包含了 map_builder_ 和 sensor_bridges_:
然后就是 map_builder_ 包含了 trajectory_builders_ 和 pose_graph_ 和 sensor_collator_.
然后 每个类后面, 都会有在我设定为2D的情况下, 具体的类的类型是什么.
里面有挺多前提假设 : collate_by_trajectory 为 false 等等.
在下面讲解代码的时候,会慢慢的进行讲解.
好的. 现在按照我了解的过程进行讲解.
也是在继续读代码的时候, 我发现第一篇我看漏了.
Node里面的构造函数是会有构造订阅主题的函数代码, 但是更重要的是在后面, 所以列出Run()的代码如下:
void Run() { .... // TrajectoryOptions在/cartographer_ros/cartographer_ros/cartographer/trajectory_options.h中定义。 TrajectoryOptions trajectory_options; // std::tie会将变量的引用整合成一个tuple，从而实现批量赋值 // 获得配置信息 std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename); // auto关键字：auto可以在声明变量的时候根据变量初始值的类型自动为此变量选择匹配的类型，类似的关键字还有decltype。举个例子：int a = 10;auto au_a = a; // 自动类型推断，au_a为int类型； cout << typeid(au_a).name() << endl auto map_builder = cartographer::common::make_unique<cartographer::mapping::MapBuilder>( node_options.map_builder_options); //Node在/cartographer_ros/cartographer_ros/cartographer/node.h中定义； // 在该构造函数中订阅了很多传感器的topic。 // 按照我看，只是发送了很多话题，没有订阅阿 Node node(node_options, std::move(map_builder), &tf_buffer); if (!FLAGS_load_state_filename.empty()) { node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } if (FLAGS_start_trajectory_with_default_topics) { node.StartTrajectoryWithDefaultTopics(trajectory_options); } //删除ros::spin() 之后的代码,节约篇章 }
上面的代码,上面我们只进行Node的构造函数的解读, 发现它会创建主题的订阅, 发布一些服务等.
但是仅有node提供服务, 是不会产生任何作用的. 因为不知道哪里进行了调用.
上一次目的性很强的进行了订阅代码的查看. 这次还是要慢慢看这个过程.
我用横线分割一下,表示上面的意思已经完了, 开始一个较为独立的部分
下面这个独立的部分, 是讲options的
// TrajectoryOptions在/cartographer_ros/cartographer_ros/cartographer/trajectory_options.h中定义。 TrajectoryOptions trajectory_options; // std::tie会将变量的引用整合成一个tuple，从而实现批量赋值 // 获得配置信息 std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
option的创建, 这个我没有细看, 只知道按照它给你的格式, 一定会得到这些选项.
对于这个过程, 我当作是黑盒, 反正是会得到.
注意的是, 配置是在两个地方有的 cartographer_ros 以及 cartographer 下的两个.
之前我一位只有cartographer_ros下一个.
我用横线分割一下,表示上面的意思已经完了, 开始一个较为独立的部分
下面这个独立的部分, 是讲map_builder_的创建的
auto map_builder = cartographer::common::make_unique<cartographer::mapping::MapBuilder>( node_options.map_builder_options);
句子的意思是, 根据 node_options 里面 map_builder_options 进行构造一个 map_builder
MapBuilder 是 cartographer mapping 里面的类. 按我的理解, 这是用来构图的一个总的类,统管各种个各样的东西.
为什么我会得到这样的理解呢? 由以下的代码给我的感觉:
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : options_(options), thread_pool_(options.num_background_threads()) { // 检查是有每这两个参数 CHECK(options.use_trajectory_builder_2d() ^ options.use_trajectory_builder_3d()); // 判断是要创建2还是3 if (options.use_trajectory_builder_2d()) { // 有两个， PoseGraph2D 和 2D 的优化问题 OptimizationProblem2D pose_graph_ = common::make_unique<PoseGraph2D>( options_.pose_graph_options(), common::make_unique<optimization::OptimizationProblem2D>( options_.pose_graph_options().optimization_problem_options()), &thread_pool_); } if (options.use_trajectory_builder_3d()) { pose_graph_ = common::make_unique<PoseGraph3D>( options_.pose_graph_options(), common::make_unique<optimization::OptimizationProblem3D>( options_.pose_graph_options().optimization_problem_options()), &thread_pool_); } // 根据选项 判断是使用哪个传感器的收集者 if (options.collate_by_trajectory()) { sensor_collator_ = common::make_unique<sensor::TrajectoryCollator>(); } else { sensor_collator_ = common::make_unique<sensor::Collator>(); } }
从代码的逻辑能够看出 这里根据2d还是3d,生成了一个 OptimizationProblem2D 的 pose_graph_.
以及一个 sensor_collator, sensor_collator_.
建议去了解一下 拉格朗日乘数法, 概率机器人对graph-base算法的概述 以及cartographer 算法的描述, 这样可以有个感性的认识.
其实可以知道 pose_graph_ 是类似的一个解决优化问题. 至于具体是什么, 我们后面再具体解决嘛, 再详细介绍.
然后 sensor::Collator ,为啥是这个, 因为我在文件夹, 找这个配置没有, 所以我觉得把这个配置为 sensor::Collator .
对于这两个的用处, 其实可以从别人的解释以及cartographer自己的注释再得到多一点认识:
// 实现称为稀疏姿势调整的循环闭包方法
// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
// on (pp. 22--29). IEEE, 2010.
// It is extended for submapping:
// Each node has been matched against one or more submaps (adding a constraint
// for each match), both poses of nodes and of submaps are to be optimized.
// All constraints are between a submap i and a node j.
// 为子图进行扩展
// 每个节点已与一个或多个子图匹配（为每个匹配添加约束）
// 节点和子图的姿势都要优化。
// 所有约束都在子图i和节点j之间。
以及
Collator,采集者，校对者，整理者, 将多传感器采集的数据归并到轨迹上。
Collator类:不可拷贝,不可赋值. 只有一个默认构造函数. 有2个数据成员
1,OrderedMultiQueue queue_; key是pair<轨迹线id,传感器id>.
一般情况下，对于已有的bag文件，轨迹id等于0.
2,std::unordered_map<int, std::vector<QueueKey>> queue_keys_
轨迹线和队列key组成的hash表,1：N模式
Collator类主要提供三个操作:
1,AddTrajectory() 添加一个轨迹线,
2,FinishTrajectory() 标记轨迹线已经采集完成
3,AddSensorData()接收传感器数据
4,Flush()刷新
https://blog.csdn.net/learnmoreonce/article/details/76218126
Maintains multiple queues of sorted sensor data and dispatches it in merge sorted order.
维护已排序的传感器数据的多个队列，并以合并排序顺序调度它。
It will wait to see at least one value for each unfinished queue before dispatching the next time ordered value across all queues.
对于每一个未完成的队列,最少需要看到最少一个值,才会分派下一个有序值
This class is thread-compatible.
这个类是线程兼容的
当时我是再深入去看了下 sensor::Collator , 花不了多少时间, 确实只是上面所说, 用来存储一些数据的类.
我们先了解到这里就好, 记住,我们的目的是了解这些类组织关系, 以便最后数据是流入哪里打好基础. 不是了解一路的所有东西.
牢记使命,不忘初心.
所以,很简单, 这句话, 就生成这样的map_builder_的东西. 这东西, 一部分是收集数据用的, 一部分用于2D优化问题.
跟我想了解的Local SLAM没有太大关系.
我用横线分割一下,表示上面的意思已经完了, 开始一个较为独立的部分
下面这个独立的部分, 是讲node的创建的
然后是之前的node的创建的代码
//Node在/cartographer_ros/cartographer_ros/cartographer/node.h中定义； // 在该构造函数中订阅了很多传感器的topic。 // 按照我看，只是发送了很多话题，没有订阅阿 Node node(node_options, std::move(map_builder), &tf_buffer);
我们还是要再进去里面的构造函数,因为里面还有一个重要的对象会被创建
Node::Node( const NodeOptions& node_options, // 选项 std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, // 地图匹配 tf2_ros::Buffer* const tf_buffer) // tf 树 : node_options_(node_options), map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { carto::common::MutexLocker lock(&mutex_); ... }
这儿最主要的是生成了一个 map_builder_bridge_ 的对象. 要注意的是, 它的初始化的时候, 包括了上面的所生成的map_builder来进行初始化
map_builder_bridge_的构造函数也是相当的简单
MapBuilderBridge::MapBuilderBridge( const NodeOptions& node_options, std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder, tf2_ros::Buffer* const tf_buffer) : node_options_(node_options), map_builder_(std::move(map_builder)), tf_buffer_(tf_buffer) {}
不过,在这,我们起码可以得出上面的一些关系, 是 map_builder_bridge 是包含了 map_builder的.
是cartographer_ros里面连接到cartographer的一个桥梁.
我用横线分割一下,表示上面的意思已经完了, 开始一个较为独立的部分
下面这个独立的部分, 是讲node.LoadState的
回到Run()函数里面的语句
if (!FLAGS_load_state_filename.empty()) { node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); }
LoadState 的作： 调用了map_builder_的成员函数LoadState来加载一个.pbstream文件。
其中loadstate是和.pbstream的文件相关，我搜索了一下，有以下的描述
Exploiting the map generated by Cartographer ROS（https://google-cartographer-ros.readthedocs.io/en/latest/assets_writer.html?highlight=.pbstream）
As sensor data come in, the state of a SLAM algorithm such as Cartographer evolves to stay the current best estimate of a robot’s trajectory and surroundings.
随着传感器数据的进入，诸如Cartographer之类的SLAM算法的状态发展为保持机器人轨迹和周围环境的当前最佳估计。
The most accurate localization and mapping Cartographer can offer is therefore the one obtained when the algorithm finishes.
因此，Cartographer可以提供的最准确的定位和映射是算法完成时获得的。
Cartographer can serialize its internal state in a .pbstream file format which is essentially a compressed protobuf file containing a snapshot of the data structures used by Cartographer internally.
制图师可以用.pbstream文件格式序列化其内部状态，该文件格式本质上是一个压缩的protobuf文件，其中包含Cartographer内部使用的数据结构的快照。
所以上面的代码，应该只是读取以前的状态。
还是很好奇以前的状态，怎么帮助之后的。
但是目前我们假设是第一次运行。没有这样的文件存在,也就是empty。跳过
我用横线分割一下,表示上面的意思已经完了, 开始一个较为独立的部分
下面这个独立的部分, 是讲node.StartTrajectoryWithDefaultTopics的
再下面依据是
if (FLAGS_start_trajectory_with_default_topics) { node.StartTrajectoryWithDefaultTopics(trajectory_options); }
之前说过,node的构造函数只是发布了各种的服务. 没有真正的启动.
直到我看到了这里, 这儿从名字就开始第一条trajectory.
不过我其实现在, 是没有运行过cartographer如何像gmapping那样的运行构图的.
所以如何实际运行的经验还是欠缺. 除非我了解了cartographer 如何开始的.
我想, 这儿是个必要的开启步骤, 开启了第一条trajectory. 但是如何一步一步循环下去进行,直到生成图.
这些是我以后要看的.
现在先来慢慢看这个函数. 这个函数涉及到挺多东西. 比较长了. 有点耐心, 一个一个看.
在开始之前, FLAGS_start_trajectory_with_default_topics 的定义是这样的
DEFINE_bool( start_trajectory_with_default_topics, true, // 启用以使用默认主题立即启动第一个轨迹 "Enable to immediately start the first trajectory with default topics.");
默认是用默认的主题来进行启动的.
来, 然后第一个是
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { carto::common::MutexLocker lock(&mutex_); CHECK(ValidateTrajectoryOptions(options)); AddTrajectory(options, DefaultSensorTopics()); }
比较简单哦. 是直接调用了 同一个类的 AddTrajectory(options, DefaultSensorTopics())
那个函数的定义是;
// 这儿是调用 LaunchSubscribers 的唯一一个 int Node::AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> expected_sensor_ids = ComputeExpectedSensorIds(options, topics); const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); LaunchSubscribers(options, topics, trajectory_id); is_active_trajectory_[trajectory_id] = true; for (const auto& sensor_id : expected_sensor_ids) { subscribed_topics_.insert(sensor_id.id); } return trajectory_id; }
比较关键的一个调用map_builder_bridge_的 AddTrajectory 函数.
先把一些比较不关键的说了 , 那么就是
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> expected_sensor_ids = ComputeExpectedSensorIds(options, topics); // 根据配置和topics 计算出一个数据结构,能够表达类型和string id ..... AddExtrapolator(trajectory_id, options); // 为 map_builder_bridge_. 添加了 Extrapolator,一个推算器 AddSensorSamplers(trajectory_id, options); // 为 map_builder_bridge_ 添加了 SensorSamplers LaunchSubscribers(options, topics, trajectory_id); // 启动订阅
因为这些都跟我看到雷达信息放进去, 还没有太大关系.
所以, 会先省略. 本篇的目的,还是搞清楚雷达信息, 如何通过不同的类弄进去的.
重要的是调用了
const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
跟入进去, 会看到
int MapBuilderBridge::AddTrajectory( const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, const TrajectoryOptions& trajectory_options) { const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, ::std::placeholders::_1, ::std::placeholders::_2, ::std::placeholders::_3, ::std::placeholders::_4, ::std::placeholders::_5)); LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; // Make sure there is no trajectory with 'trajectory_id' yet. CHECK_EQ(sensor_bridges_.count(trajectory_id), 0); sensor_bridges_[trajectory_id] = cartographer::common::make_unique<SensorBridge>( trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_->GetTrajectoryBuilder(trajectory_id)); auto emplace_result = trajectory_options_.emplace(trajectory_id, trajectory_options); CHECK(emplace_result.second == true); return trajectory_id; }
这里主要有两个函数, 调用的是 map_builder_->AddTrajectoryBuilder() 的函数
以及 SensorBridge 的函数.
首先是 AddTrajectoryBuilder. 已经是通过cartographer::maping里面的函数了
const int trajectory_id = map_builder_->AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options, ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this, ::std::placeholders::_1, ::std::placeholders::_2, ::std::placeholders::_3, ::std::placeholders::_4, ::std::placeholders::_5));
这个函数如果直接按跳转,会跳转到虚类, 但是经过前面的分析, 在2d的时候, 是 cartographer::mapping::MapBuilder
这个函数的定义是
// 创建一个新的TrajectoryBuilder并返回它的trajectory_id. int MapBuilder::AddTrajectoryBuilder( const std::set<SensorId>& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) { // 生成trajectory_id，trajectory_builders_是一个向量，存着所有的trajectory。 // 因此，当有一个新的trajectory时，以向量的size值作为新的trajectory，加入到trajectory_builders_中 const int trajectory_id = trajectory_builders_.size(); { // 如果是2d的情况 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))); if (trajectory_options.has_overlapping_submaps_trimmer_2d()) { const auto& trimmer_options = trajectory_options.overlapping_submaps_trimmer_2d(); pose_graph_->AddTrimmer(common::make_unique<OverlappingSubmapsTrimmer2D>( trimmer_options.fresh_submaps_count(), trimmer_options.min_covered_area() / common::Pow2(trajectory_options.trajectory_builder_2d_options() .submaps_options() .grid_options_2d() .resolution()), trimmer_options.min_added_submaps_count())); } }
if (trajectory_options.pure_localization()) { constexpr int kSubmapsToKeep = 3; pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>( trajectory_id, kSubmapsToKeep)); } if (trajectory_options.has_initial_trajectory_pose()) { const auto& initial_trajectory_pose = // 返回一个初始的 initial_trajectory_pose trajectory_options.initial_trajectory_pose(); //调用pose_graph_中的方法，设置初始pose。跟全局相关的事情，都交给pose_graph_来处理。 pose_graph_->SetInitialTrajectoryPose( trajectory_id, initial_trajectory_pose.to_trajectory_id(), transform::ToRigid3(initial_trajectory_pose.relative_pose()), common::FromUniversal(initial_trajectory_pose.timestamp())); } proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto; // 对于每一个 expected_sensor_ids ，添加金option里面 for (const auto& sensor_id : expected_sensor_ids) { *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id); } *options_with_sensor_ids_proto.mutable_trajectory_builder_options() = trajectory_options; all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto); CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size()); return trajectory_id; }
代码分3d和2d, 我把3d的删除, 只留了2d
这儿,主要和数据相关的, 是以下两个变量:
local_trajectory_builder, trajectory_builders:
其他的是后面的全局的.以后再去了解吧.
从LocalTrajectoryBuilder2D的头文件和构造函数,能够看到,这里是进行局部slam的主要地方
这儿只列出了构造函数
LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(//为几个成员变量初始化 const proto::LocalTrajectoryBuilderOptions2D& options, const std::vector<std::string>& expected_range_sensor_ids) : options_(options), active_submaps_(options.submaps_options()), motion_filter_(options_.motion_filter_options()), real_time_correlative_scan_matcher_( options_.real_time_correlative_scan_matcher_options()), ceres_scan_matcher_(options_.ceres_scan_matcher_options()), range_data_collator_(expected_range_sensor_ids) {}
从之前了解的东西，可以知道，这儿包含完成了局部SLAM的所有东西。
包括了下面:
options_ ： 选项
active_submaps_：一个包含两个map东西，一个构造，一个对比，class ActiveSubmaps2D
motion_filter_：过滤行动，
real_time_correlative_scan_matcher_：一个scan matching
ceres_scan_matcher_:一个scan matching，
range_data_collator_：数据的收集者， class RangeDataCollator
看到这里，我似乎懂了local slam的过程了
可能是data进来，会进来这个 range_data_collator_ 里面保存，
然后 InsertRangeData，会把插进子图
但目前,只是猜测. 后面会去确认的, 只是现在只是想直到数据是怎么流进去.
从这儿, 我们是可以大概猜测, local slam 是数据流入的一部分收集地.
然后就是 trajectory_builders这个数组, 这个主要是 CollatedTrajectoryBuilder 这个类型的指针的数组.
我们需要进一步去看下 CollatedTrajectoryBuilder 这个,到底拥有什么变量
GlobalTrajectoryBuilder( std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder, const int trajectory_id, PoseGraph* const pose_graph, const LocalSlamResultCallback& local_slam_result_callback) : trajectory_id_(trajectory_id), pose_graph_(pose_graph), local_trajectory_builder_(std::move(local_trajectory_builder)), local_slam_result_callback_(local_slam_result_callback) {} ~GlobalTrajectoryBuilder() override {}
从构造函数,传入进去的, 就能清除看到 它包含了
pose_graph_(pose_graph)
local_trajectory_builder_(std::move(local_trajectory_builder))
当前只要直到组织. 所以, 到这就先完事了
然后就是下一句了:
是不是已经昏了头了. 我也是, 所以我总结了这些东西的跳转如下:
Node::StartTrajectoryWithDefaultTopics Node::LaunchSubscribers 曾经说过, 这是 Node::AddSensorSamplers Node::AddExtrapolator Node::AddTrajectory() map_builder_bridge_::AddTrajectory( map_builder_::AddTrajectoryBuilder: LocalTrajectoryBuilder2D() CreateGlobalTrajectoryBuilder2D() (说到了这,所以下一个会去说SensorBridge了) pose_graph_->AddTrimmer(common::make_unique<OverlappingSubmapsTrimmer2D> pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer> pose_graph_->SetInitialTrajectoryPose( cartographer::common::make_unique<SensorBridge> map_builder_->GetTrajectoryBuilder(trajectory_id)
原本的语句是下面这样的.
sensor_bridges_[trajectory_id] = cartographer::common::make_unique<SensorBridge>( trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_->GetTrajectoryBuilder(trajectory_id));
所以会构造一个 SensorBridge , 调用构造函数, 它的构造函数如下:
SensorBridge::SensorBridge( const int num_subdivisions_per_laser_scan, const std::string& tracking_frame, const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer, carto::mapping::TrajectoryBuilderInterface* const trajectory_builder) : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan), tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), trajectory_builder_(trajectory_builder) {}
所以,这里已经能够得到它的组织关系了
GetTrajectoryBuilder(), 其实是获得了 trajectory_builders_ 的其中一个的指针.
所以,最后得出了组织关系是这样的:
node: class Node sensor_samplers_: extrapolators_: map_builder_bridge_: map_builder_: cartographer::mapping::MapBuilder trajectory_builders_: CreateGlobalTrajectoryBuilder2D(), CollatedTrajectoryBuilder pose_graph_: OptimizationProblem2D sensor_collator_: Collator OrderedMultiQueue sensor_bridges_: SensorBridge trajectory_builder_: class CollatedTrajectoryBuilder GlobalTrajectoryBuilder: 继承于 TrajectoryBuilderInterface local_trajectory_builder_: LocalTrajectoryBuilder2D pose_graph_: PoseGraph2D local_slam_result_callback_: 回调函数 trajectory_id_: 当前的trajectory_id
为什么要得到这些呢,是在回调函数当中,弄清楚, 到底调用的是哪个.
从这些组织中, 我绝对,对以后了解整个流程是怎么起作用的.
简单讲解以下.
可以看到, 最后 local_trajectory_builder_ 是Local SLAM的主力.
到底是怎么看出来的, 从后面 handlelaserMSG中,可以直到保存在哪.
我也只是猜测. 保存是保存在那了.
我们文章3, 就说这个话题哦.
利用这些组织关系, 说明数据流到那儿.
然后3之后, 我会具体看LocalSLAM是如何进行的.
