從上一篇開始, 我很着急地去希望看到雷達信息是如訂閱的. 只要有了訂閱的開始, 那么數據就會被回調到回調函數,才知道那些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是如何進行的.
