Cartographer原始碼閱讀(6):LocalTrajectoryBuilder和PoseExtrapolator
LocalTrajectoryBuilder意思是區域性軌跡的構建,下面的類圖中方法的引數沒有畫進去。
注意其中的三個類:PoseExtrapolator類,RealTimeCorrelativeScanMatcher類和CeresScanMatcher類。
(1)PoseExtrapolator類(如下圖),Node類和LocalTrajectoryBuilder類都有用到PoseExtrapolator物件,好像兩者之間並沒有什麼關係?
LocalTrajectoryBuilder中的PoseExtrapolator物件類似於運動模型。
(Node類中的可能是為了釋出位姿資訊用的,單獨進行了位姿推算。先不管了。)
PoseExtrapolator的建構函式 VS 通過IMU初始化InitializeWithImu方法。
在LocalTrajectoryBuilder::InitializeExtrapolator中對其建構函式的呼叫:
1 void LocalTrajectoryBuilder::InitializeExtrapolator(const common::Time time) 2 { 3 if (extrapolator_ != nullptr) { 4 return; 5 } 6 // We derive velocities from poses which are at least 1 ms apart for numerical7 // stability. Usually poses known to the extrapolator will be further apart 8 // in time and thus the last two are used. 9 constexpr double kExtrapolationEstimationTimeSec = 0.001; 10 // TODO(gaschler): Consider using InitializeWithImu as 3D does. 11 extrapolator_ = common::make_unique<mapping::PoseExtrapolator>(12 ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), 13 options_.imu_gravity_time_constant()); 14 extrapolator_->AddPose(time, transform::Rigid3d::Identity()); 15 }
PoseExtrapolator::InitializeWithImu方法:
1 std::unique_ptr<PoseExtrapolator> PoseExtrapolator::InitializeWithImu( 2 const common::Duration pose_queue_duration, 3 const double imu_gravity_time_constant, const sensor::ImuData& imu_data) 4 { 5 auto extrapolator = common::make_unique<PoseExtrapolator>(pose_queue_duration, imu_gravity_time_constant); 6 extrapolator->AddImuData(imu_data); 7 extrapolator->imu_tracker_ =common::make_unique<ImuTracker>(imu_gravity_time_constant, imu_data.time); 8 extrapolator->imu_tracker_->AddImuLinearAccelerationObservation( 9 imu_data.linear_acceleration); 10 extrapolator->imu_tracker_->AddImuAngularVelocityObservation( 11 imu_data.angular_velocity); 12 extrapolator->imu_tracker_->Advance(imu_data.time); 13 extrapolator->AddPose(imu_data.time,transform::Rigid3d::Rotation(extrapolator->imu_tracker_->orientation())); 14 return extrapolator; 15 }
LocalTrajectoryBuilder的AddImuData和AddOdometryData方法不贅述。
1 void LocalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) { 2 CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added."; 3 InitializeExtrapolator(imu_data.time); 4 extrapolator_->AddImuData(imu_data); 5 } 6 7 void LocalTrajectoryBuilder::AddOdometryData( 8 const sensor::OdometryData& odometry_data) { 9 if (extrapolator_ == nullptr) { 10 // Until we've initialized the extrapolator we cannot add odometry data. 11 LOG(INFO) << "Extrapolator not yet initialized."; 12 return; 13 } 14 extrapolator_->AddOdometryData(odometry_data); 15 }
如下檢視LocalTrajectoryBuilder::AddRangeData方法。如果使用IMU資料,直接進入10行,如果不是用進入7行。
1 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult> 2 LocalTrajectoryBuilder::AddRangeData(const common::Time time, 3 const sensor::TimedRangeData& range_data) 4 { 5 // Initialize extrapolator now if we do not ever use an IMU. 6 if (!options_.use_imu_data()) 7 { 8 InitializeExtrapolator(time); 9 } 10 if (extrapolator_ == nullptr) 11 { 12 // Until we've initialized the extrapolator with our first IMU message, we 13 // cannot compute the orientation of the rangefinder. 14 LOG(INFO) << "Extrapolator not yet initialized."; 15 return nullptr; 16 } 17 18 CHECK(!range_data.returns.empty()); 19 CHECK_EQ(range_data.returns.back()[3], 0); 20 const common::Time time_first_point = 21 time + common::FromSeconds(range_data.returns.front()[3]); 22 if (time_first_point < extrapolator_->GetLastPoseTime()) { 23 LOG(INFO) << "Extrapolator is still initializing."; 24 return nullptr; 25 } 26 27 std::vector<transform::Rigid3f> range_data_poses; 28 range_data_poses.reserve(range_data.returns.size()); 29 for (const Eigen::Vector4f& hit : range_data.returns) { 30 const common::Time time_point = time + common::FromSeconds(hit[3]); 31 range_data_poses.push_back( 32 extrapolator_->ExtrapolatePose(time_point).cast<float>()); 33 } 34 35 if (num_accumulated_ == 0) { 36 // 'accumulated_range_data_.origin' is uninitialized until the last 37 // accumulation. 38 accumulated_range_data_ = sensor::RangeData{{}, {}, {}}; 39 } 40 41 // Drop any returns below the minimum range and convert returns beyond the 42 // maximum range into misses. 43 for (size_t i = 0; i < range_data.returns.size(); ++i) { 44 const Eigen::Vector4f& hit = range_data.returns[i]; 45 const Eigen::Vector3f origin_in_local = 46 range_data_poses[i] * range_data.origin; 47 const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>(); 48 const Eigen::Vector3f delta = hit_in_local - origin_in_local; 49 const float range = delta.norm(); 50 if (range >= options_.min_range()) { 51 if (range <= options_.max_range()) { 52 accumulated_range_data_.returns.push_back(hit_in_local); 53 } else { 54 accumulated_range_data_.misses.push_back( 55 origin_in_local + 56 options_.missing_data_ray_length() / range * delta); 57 } 58 } 59 } 60 ++num_accumulated_; 61 62 if (num_accumulated_ >= options_.num_accumulated_range_data()) { 63 num_accumulated_ = 0; 64 const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( 65 extrapolator_->EstimateGravityOrientation(time)); 66 accumulated_range_data_.origin = 67 range_data_poses.back() * range_data.origin; 68 return AddAccumulatedRangeData( 69 time, 70 TransformToGravityAlignedFrameAndFilter( 71 gravity_alignment.cast<float>() * range_data_poses.back().inverse(), 72 accumulated_range_data_), 73 gravity_alignment); 74 } 75 return nullptr; 76 }
接著,LocalTrajectoryBuilder::AddAccumulatedRangeData程式碼如下,傳入的引數為3個。
const common::Time time,const sensor::RangeData& gravity_aligned_range_data, const transform::Rigid3d& gravity_alignment
重力定向,定向後的深度資料和定向矩陣。
注意下面21行程式碼執行了掃描匹配的ScanMatch方法,之後程式碼29行呼叫的extrapolator_->AddPose()方法:
每次掃描匹配之後執行AddPose方法。
1 std::unique_ptr<LocalTrajectoryBuilder::MatchingResult> 2 LocalTrajectoryBuilder::AddAccumulatedRangeData( 3 const common::Time time, 4 const sensor::RangeData& gravity_aligned_range_data, 5 const transform::Rigid3d& gravity_alignment) 6 { 7 if (gravity_aligned_range_data.returns.empty()) 8 { 9 LOG(WARNING) << "Dropped empty horizontal range data."; 10 return nullptr; 11 } 12 13 // Computes a gravity aligned pose prediction. 14 const transform::Rigid3d non_gravity_aligned_pose_prediction = 15 extrapolator_->ExtrapolatePose(time); 16 const transform::Rigid2d pose_prediction = transform::Project2D( 17 non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); 18 19 // local map frame <- gravity-aligned frame 20 std::unique_ptr<transform::Rigid2d> pose_estimate_2d = 21 ScanMatch(time, pose_prediction, gravity_aligned_range_data); 22 if (pose_estimate_2d == nullptr) 23 { 24 LOG(WARNING) << "Scan matching failed."; 25 return nullptr; 26 } 27 const transform::Rigid3d pose_estimate = 28 transform::Embed3D(*pose_estimate_2d) * gravity_alignment; 29 extrapolator_->AddPose(time, pose_estimate); 30 31 sensor::RangeData range_data_in_local = 32 TransformRangeData(gravity_aligned_range_data, 33 transform::Embed3D(pose_estimate_2d->cast<float>())); 34 std::unique_ptr<InsertionResult> insertion_result = 35 InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data, 36 pose_estimate, gravity_alignment.rotation()); 37 return common::make_unique<MatchingResult>( 38 MatchingResult{time, pose_estimate, std::move(range_data_in_local), 39 std::move(insertion_result)}); 40 } 41 42 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> 43 LocalTrajectoryBuilder::InsertIntoSubmap( 44 const common::Time time, const sensor::RangeData& range_data_in_local, 45 const sensor::RangeData& gravity_aligned_range_data, 46 const transform::Rigid3d& pose_estimate, 47 const Eigen::Quaterniond& gravity_alignment) 48 { 49 if (motion_filter_.IsSimilar(time, pose_estimate)) 50 { 51 return nullptr; 52 } 53 54 // Querying the active submaps must be done here before calling 55 // InsertRangeData() since the queried values are valid for next insertion. 56 std::vector<std::shared_ptr<const Submap>> insertion_submaps; 57 for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) 58 { 59 insertion_submaps.push_back(submap); 60 } 61 active_submaps_.InsertRangeData(range_data_in_local); 62 63 sensor::AdaptiveVoxelFilter adaptive_voxel_filter( 64 options_.loop_closure_adaptive_voxel_filter_options()); 65 const sensor::PointCloud filtered_gravity_aligned_point_cloud = 66 adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns); 67 68 return common::make_unique<InsertionResult>(InsertionResult{ 69 std::make_shared<const mapping::TrajectoryNode::Data>( 70 mapping::TrajectoryNode::Data{ 71 time, 72 gravity_alignment, 73 filtered_gravity_aligned_point_cloud, 74 {}, // 'high_resolution_point_cloud' is only used in 3D. 75 {}, // 'low_resolution_point_cloud' is only used in 3D. 76 {}, // 'rotational_scan_matcher_histogram' is only used in 3D. 77 pose_estimate}), 78 std::move(insertion_submaps)}); 79 }LocalTrajectoryBuilder::AddAccumulatedRangeData
(2)RealTimeCorrelativeScanMatcher類,實時的掃描匹配,用的相關分析方法。
相關推薦
Cartographer原始碼閱讀(6):LocalTrajectoryBuilder和PoseExtrapolator
LocalTrajectoryBuilder意思是區域性軌跡的構建,下面的類圖中方法的引數沒有畫進去。 注意其中的三個類:PoseExtrapolator類,RealTimeCorrelativeScanMatcher類和CeresScanMatcher類。 (1)PoseExtrapolator類(
Cartographer原始碼閱讀(2):Node和MapBuilder物件
上文提到特別注意map_builder_bridge_.AddTrajectory(x,x),檢視其中的程式碼。兩點: 首先是map_builder_.AddTrajectoryBuilder(...),呼叫了map_builder_物件的方法。其次是sensor_bridges_鍵值對的賦值。
Cartographer原始碼閱讀(4):Node和MapBuilder物件2
MapBuilder的成員變數sensor::Collator sensor_collator_; 再次閱讀MapBuilder::AddTrajectoryBuilder方法。首先構造了mapping::GlobalTrajectoryBuilder例項,接著作為引數構造了CollatedTraj
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,
struts1原始碼學習6(doPost和doGet)
ActionServlet中的doPost和doGet的程式碼是一樣的,都是呼叫process 直接看process程式碼 protected void process(HttpServletRequest request, HttpServletResponse re
Caffe原始碼解析6:Neuron_Layer
NeuronLayer,顧名思義這裡就是神經元,啟用函式的相應層。我們知道在blob進入啟用函式之前和之後他的size是不會變的,而且啟用值也就是輸出 \(y\) 只依賴於相應的輸入 \(x\)。在Caffe裡面所有的layer的實現都放在src資料夾下的layer資料夾中,基本上很多文章裡應用到的laye
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反射,理解常用的設計模式。當然,這些複習筆記也會在今後的複習中
ruby gem bacon原始碼閱讀6
ruby gem bacon原始碼閱讀6 今天早上看些雜書,讀到《大學之路》中吳軍講,人生是馬拉松,因為很多人畢業後就不學習了,只要學習就有收穫。感覺很深 ,於是接著讀原始碼。靜下心來。 用法是: require 'bacon' &
caffe程式碼閱讀6:Filler的實現細節-2016.3.18
一、Filler的作用簡介 Filler層的作用實際上就是根據proto中給出的引數對權重進行初始化,初始化的方式有很多種,分別為常量初始化(constant)、高斯分佈初始化(gaussian)、positive_unitball初始化、均勻分佈初始化(uniform)
原始碼閱讀系列:為什麼要閱讀原始碼?
一.為什麼要閱讀程式碼 養成閱讀高品質程式碼的習慣,可以提高編寫程式碼的能力。 電腦科學是一門實踐性很強的學科,很多內容在書本上根本學不到。就拿專案的組織來說,沒有什麼書籍專門論述應該如何組織與管理專案的目錄結構,因為這本身就是一種見仁見智的活動,要