Cartographer原始碼閱讀(4):Node和MapBuilder物件2
MapBuilder的成員變數sensor::Collator sensor_collator_;
再次閱讀MapBuilder::AddTrajectoryBuilder方法。首先構造了mapping::GlobalTrajectoryBuilder例項,接著作為引數構造了CollatedTrajectoryBuilder例項。
trajectory_builders_.push_back( common::make_unique<CollatedTrajectoryBuilder>( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique<mapping::GlobalTrajectoryBuilder<mapping_2d::LocalTrajectoryBuilder,mapping_2d::proto::LocalTrajectoryBuilderOptions,mapping_2d::PoseGraph>> (trajectory_options.trajectory_builder_2d_options(),trajectory_id, pose_graph_2d_.get(),local_slam_result_callback) ) );
這裡sensor_collator_作為引數傳入,參與CollatedTrajectoryBuilder構造。檢視建構函式:
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(sensor::Collator* const sensor_collator, const int trajectory_id, const std::unordered_set<std::string>& 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()) { sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_ids, [this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { HandleCollatedSensorData(sensor_id, std::move(data)); }); }
這裡是回撥函式,std::unique_ptr是表示引數為智慧指標。
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) { HandleCollatedSensorData(sensor_id, std::move(data)); }
(1)檢視sensor::Collator的AddTrajectory方法:
void 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); } }
for (const auto& sensor_id : expected_sensor_ids)用到了C++11的auto新特性。
(2)檢視HandleCollatedSensorData方法。呼叫了data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());這裡wrapped_trajectory_builder_是在CollatedTrajectoryBuilder建構函式中賦值的。為GlobalTrajectoryBuilder物件。因而檢視sensor::Data的AddToTrajectoryBuilder() 方法。
virtual void AddToTrajectoryBuilder(mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;是sensor::Data類的一個虛方法。內部執行了trajectory_builder->AddSensorData(sensor_id_, data_);
最後呼叫的是GlobalTrajectoryBuilder物件的AddSensorData(xx)方法。
1 void CollatedTrajectoryBuilder::HandleCollatedSensorData( const std::string& sensor_id, std::unique_ptr<sensor::Data> data) 2 { 3 auto it = rate_timers_.find(sensor_id); 4 if (it == rate_timers_.end()) 5 { 6 it = rate_timers_ .emplace( 7 std::piecewise_construct, std::forward_as_tuple(sensor_id), 8 std::forward_as_tuple(common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first; 9 } 10 it->second.Pulse(data->GetTime()); 11 12 if (std::chrono::steady_clock::now() - last_logging_time_ > 13 common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) 14 { 15 for (const auto& pair : rate_timers_) 16 { 17 LOG(INFO) << pair.first << " rate: " << pair.second.DebugString(); 18 } 19 last_logging_time_ = std::chrono::steady_clock::now(); 20 } 21 22 data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get()); 23 } 24 25 }CollatedTrajectoryBuilder::HandleCollatedSensorData
template <typename DataType> class Dispatchable : public Data { public: Dispatchable(const std::string &sensor_id, const DataType &data): Data(sensor_id), data_(data) {} common::Time GetTime() const override { return data_.time; } void AddToTrajectoryBuilder( mapping::TrajectoryBuilderInterface *const trajectory_builder) override { trajectory_builder->AddSensorData(sensor_id_, data_); } private: const DataType data_; };
再以IMU資料為例,GlobalTrajectoryBuilder類的AddSensorData(xx):
void AddSensorData(const std::string& sensor_id, const sensor::ImuData& imu_data) override { local_trajectory_builder_.AddImuData(imu_data); pose_graph_->AddImuData(trajectory_id_, imu_data); }
再看一下鐳射點雲的資料
1 void AddSensorData( const std::string& sensor_id, const sensor::TimedPointCloudData& timed_point_cloud_data) override 2 { 3 std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result = 4 local_trajectory_builder_.AddRangeData( timed_point_cloud_data.time, 5 sensor::TimedRangeData {timed_point_cloud_data.origin, 6 timed_point_cloud_data.ranges, {}} 7 ); 8 if (matching_result == nullptr) 9 { 10 // The range data has not been fully accumulated yet. 11 return; 12 } 13 std::unique_ptr<mapping::NodeId> node_id; 14 if (matching_result->insertion_result != nullptr) 15 { 16 node_id = ::cartographer::common::make_unique<mapping::NodeId>( 17 pose_graph_->AddNode(matching_result->insertion_result->constant_data, 18 trajectory_id_, matching_result->insertion_result->insertion_submaps)); 19 CHECK_EQ(node_id->trajectory_id, trajectory_id_); 20 } 21 if (local_slam_result_callback_) 22 { 23 local_slam_result_callback_( trajectory_id_, matching_result->time, 24 matching_result->local_pose, 25 std::move(matching_result->range_data_in_local), std::move(node_id)); 26 } 27 }
這裡有兩個重要的步驟一個是local_trajectory_builder_.AddRangeData(xxx),一個是 pose_graph_->AddNode(xxx)方法。同時std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matching_result作為AddNode方法的引數。
1 mapping::NodeId PoseGraph::AddNode( 2 std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, 3 const int trajectory_id, 4 const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { 5 const transform::Rigid3d optimized_pose( 6 GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); 7 8 common::MutexLocker locker(&mutex_); 9 AddTrajectoryIfNeeded(trajectory_id); 10 const mapping::NodeId node_id = trajectory_nodes_.Append( 11 trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); 12 ++num_trajectory_nodes_; 13 14 // Test if the 'insertion_submap.back()' is one we never saw before. 15 if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 || 16 std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap != 17 insertion_submaps.back()) { 18 // We grow 'submap_data_' as needed. This code assumes that the first 19 // time we see a new submap is as 'insertion_submaps.back()'. 20 const mapping::SubmapId submap_id = 21 submap_data_.Append(trajectory_id, SubmapData()); 22 submap_data_.at(submap_id).submap = insertion_submaps.back(); 23 } 24 25 // We have to check this here, because it might have changed by the time we 26 // execute the lambda. 27 const bool newly_finished_submap = insertion_submaps.front()->finished(); 28 AddWorkItem([=]() REQUIRES(mutex_) { 29 ComputeConstraintsForNode(node_id, insertion_submaps, 30 newly_finished_submap); 31 }); 32 return node_id; 33 }PoseGraph::AddNode
PoseGraph::AddNode方法很重要,分析節點和子圖的關係。
此處強調一下GlobalTrajectoryBuilder的兩個關鍵物件local_trajectory_builder_和pose_graph_。
PoseGraph* const pose_graph_; LocalTrajectoryBuilder local_trajectory_builder_;
接下來按照準備安裝ROS訊息釋出和處理的流程進行分析,即資料流。
參考資料:
相關推薦
Cartographer原始碼閱讀(4):Node和MapBuilder物件2
MapBuilder的成員變數sensor::Collator sensor_collator_; 再次閱讀MapBuilder::AddTrajectoryBuilder方法。首先構造了mapping::GlobalTrajectoryBuilder例項,接著作為引數構造了CollatedTraj
Cartographer原始碼閱讀(2):Node和MapBuilder物件
上文提到特別注意map_builder_bridge_.AddTrajectory(x,x),檢視其中的程式碼。兩點: 首先是map_builder_.AddTrajectoryBuilder(...),呼叫了map_builder_物件的方法。其次是sensor_bridges_鍵值對的賦值。
Cartographer原始碼閱讀(6):LocalTrajectoryBuilder和PoseExtrapolator
LocalTrajectoryBuilder意思是區域性軌跡的構建,下面的類圖中方法的引數沒有畫進去。 注意其中的三個類:PoseExtrapolator類,RealTimeCorrelativeScanMatcher類和CeresScanMatcher類。 (1)PoseExtrapolator類(
Cartographer原始碼閱讀(7):軌跡推算和位姿推算的原理
其實也就是包括兩個方面的內容:類似於運動模型的位姿估計和掃描匹配,因為需要計算速度,所以時間就有必要了! 1. PoseExtrapolator解決了IMU資料、里程計和位姿資訊進行融合的問題。 該類定義了三個佇列。 1 std::deque<TimedPose> timed_pose_
Cartographer原始碼閱讀(8):imu_tracker
1 /* 2 * Copyright 2016 The Cartographer Authors 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use
Cartographer原始碼閱讀(1):程式入口
1 [email protected]:~$ sudo apt-get install kdevelop 2 [sudo] password for yhexie: 3 Reading package lists... Done 4 Building dependency
Cartographer原始碼閱讀(5):PoseGraph位姿圖
PoseGraph位姿圖 mapping2D::PoseGraph類的註釋: // Implements the loop closure method called Sparse Pose Adjustment (SPA) from// Konolige, Kurt, et al. "E
Cartographer原始碼閱讀(3):程式邏輯結構
Cartographer早期的程式碼在進行3d製圖的時候使用了UKF方法,檢視現有的tag版本,可以轉到0.1.0和0.2.0檢視,包含kalman_filter資料夾。 資料夾中的pose_tracker類檔案在mapping_3d的檔案加下有kalman_local_trajectory_builder
Cartographer原始碼閱讀(9):圖優化的前端——閉環檢測
約束計算 閉環檢測的策略:搜尋閉環,通過匹配檢測是否是閉環,採用了分支定界法。 前已經述及PoseGraph的內容,此處繼續。位姿圖類定義了pose_graph::ConstraintBuilder constraint_builder_物件。 1.ConstraintBuilder類圖 定義了S
Ethzasl MSF原始碼閱讀(3):MSF_Core和PoseMeasurement
1.MSF_Core的三個函式:ProcessIMU、ProcessExternallyPropagatedState和AddMeasurement MSF_Core維護了狀態佇列和觀測值佇列,這裡需要結合論文思考這個狀態佇列的作用。 ProcessIMU方法: 1 template<
Ethzasl MSF原始碼閱讀(1):程式入口和主題訂閱
Ethz的Stephen Weiss的工作,是一個IMU鬆耦合的方法。 1.程式入口:ethzasl_msf\msf_updates\src\pose_msf\main.cpp 1 #include "pose_sensormanager.h" 2 3 int main(int argc,
node.js零基礎詳細教程(4):node.js事件機制、node異步IO操作
nod server nbsp node i++ 兩個 con 錯誤 定時器 第四章 建議學習時間3小時 課程共10章 學習方式:詳細閱讀,並手動實現相關代碼 學習目標:此教程將教會大家 安裝Node、搭建服務器、express、mysql、mongodb、編寫後臺業務邏
第11章 拾遺4:IPv6和IPv4共存技術(1)_雙棧技術和6to4隧道技術
說明 images 測試結果 ges conf alt style dns服務 數據 6. IPv6和IPv4共存技術 6.1 雙棧技術 (1)雙協議主機的協議結構 (2)雙協議棧示意圖 ①雙協議主機在通信時首先通過支持雙協議的DNS服務器查詢與目的主機名對應的
實驗4:[bx]和loop的使用
實驗任務一 綜合使用 loop,[bx],編寫完整彙編程式,實現向記憶體 b800:07b8 開始的連續 16 個字單元重複填充字資料 0403H 實現: 1.編寫原始碼檔案:test1.asm 注意點:在彙編程式中,資料不能以字母開頭。b800前要加上0。 2.編譯、連結 批處理檔案:p
實驗 4:樹和二叉樹的實驗 1
一、實驗目的 1、 熟練理解樹和二叉樹的相關概念,掌握的儲存結構和相關操作實現; 2、 掌握樹的順序結構的實現; 3、 學會運用樹的知識解決實際問題。 二、實驗內容 自己確定一個二叉樹(樹結點型別、數目和結構自定)利用順序結構方法儲存。 實現樹的構造,並完成:
Caffe原始碼解析4: Data_layer
data_layer應該是網路的最底層,主要是將資料送給blob進入到net中,在data_layer中存在多個跟data_layer相關的類 BaseDataLayer BasePrefetchingDataLayer DataLayer DummyDataLayer HDF5DataLayer HDF
Spring4.3.12原始碼閱讀系列:1-環境搭建
學習任務 近期想增加部分原始碼閱讀經驗,提高自己在造輪子方面的實力,增長些在設計模式應用方面的編碼能力,以及懷著向大佬們膜拜的心情,開始有計劃地閱讀Spring原始碼 前期準備 以下幾項準備事項,算是基本的日常開發環境,就算沒有,也是動動手很快安
原始碼分析之: WebSocket 和 是如何將收發到的訊息投遞給cocos主執行緒的
-->websocket的3種使用場景: 1)h5瀏覽器中websocket由瀏覽器提供 2)node.js中,可以使用ws模組寫伺服器 3)native app中,可以使用c++版本的websocket匯出c++介面給cocos creator客戶端使用
Tensorflow object detection API 原始碼閱讀筆記:RPN
Update: 建議先看從程式設計實現角度學習Faster R-CNN,比較直觀。這裡由於原始碼抽象程度較高,顯得比較混亂。 faster_rcnn_meta_arch.py中這兩個對應知乎文章中RPN包含的3*3和1*1卷積: rpn_box_pred
Spring原始碼閱讀——Bean的載入和獲取過程
我們經常使用Spring,並且也都瞭解其大概原理。我想我們一定會對Spring原始碼的解讀有迫切的渴望。 我也如此。所以,我打算閱讀一下Spring的原始碼。再此之前,我也為此準備了很多。包括,去複習熟練java反射,理解常用的設計模式。當然,這些複習筆記也會在今後的複習中