Cartographer原始碼閱讀(5):PoseGraph位姿圖
PoseGraph位姿圖
mapping2D::PoseGraph類的註釋:
// Implements the loop closure method called Sparse Pose Adjustment (SPA) from |
1.繼承關係mapping,mapping2D,mapping3D (以前叫sparse_pose_graph.h,目前名稱被修改了)
類中一個很重要的類OptimizationProblem物件optimization_problem_,該類的註釋是Implements the SPA loop closure method.即實現了SPA方法的閉環。
類的主要成員變數和成員函式如類圖所示,其中重要的方法PoseGraph::RunOptimization()。
pose_graph::OptimizationProblem optimization_problem_;
pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
接上一篇在PoseGraph::AddNode方法中呼叫了 AddTrajectoryIfNeeded() 和 PoseGraph::ComputeConstraintsForNode()方法。
1 void PoseGraph::ComputeConstraintsForNode( 2 const mapping::NodeId& node_id, 3 std::vector<std::shared_ptr<const Submap>> insertion_submaps, 4 const bool newly_finished_submap) {5 const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; 6 const std::vector<mapping::SubmapId> submap_ids = InitializeGlobalSubmapPoses( 7 node_id.trajectory_id, constant_data->time, insertion_submaps); 8 CHECK_EQ(submap_ids.size(), insertion_submaps.size()); 9 const mapping::SubmapId matching_id = submap_ids.front(); 10 const transform::Rigid2d pose = transform::Project2D( 11 constant_data->local_pose * 12 transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); 13 const transform::Rigid2d optimized_pose = 14 optimization_problem_.submap_data().at(matching_id).global_pose * 15 pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() * 16 pose; 17 optimization_problem_.AddTrajectoryNode( 18 matching_id.trajectory_id, constant_data->time, pose, optimized_pose, 19 constant_data->gravity_alignment); 20 for (size_t i = 0; i < insertion_submaps.size(); ++i) { 21 const mapping::SubmapId submap_id = submap_ids[i]; 22 // Even if this was the last node added to 'submap_id', the submap will only 23 // be marked as finished in 'submap_data_' further below. 24 CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); 25 submap_data_.at(submap_id).node_ids.emplace(node_id); 26 const transform::Rigid2d constraint_transform = 27 pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose; 28 constraints_.push_back(Constraint{submap_id, 29 node_id, 30 {transform::Embed3D(constraint_transform), 31 options_.matcher_translation_weight(), 32 options_.matcher_rotation_weight()}, 33 Constraint::INTRA_SUBMAP}); 34 } 35 36 for (const auto& submap_id_data : submap_data_) { 37 if (submap_id_data.data.state == SubmapState::kFinished) { 38 CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); 39 ComputeConstraint(node_id, submap_id_data.id); 40 } 41 } 42 43 if (newly_finished_submap) { 44 const mapping::SubmapId finished_submap_id = submap_ids.front(); 45 SubmapData& finished_submap_data = submap_data_.at(finished_submap_id); 46 CHECK(finished_submap_data.state == SubmapState::kActive); 47 finished_submap_data.state = SubmapState::kFinished; 48 // We have a new completed submap, so we look into adding constraints for 49 // old nodes. 50 ComputeConstraintsForOldNodes(finished_submap_id); 51 } 52 constraint_builder_.NotifyEndOfNode(); 53 ++num_nodes_since_last_loop_closure_; 54 if (options_.optimize_every_n_nodes() > 0 && 55 num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) { 56 CHECK(!run_loop_closure_); 57 run_loop_closure_ = true; 58 // If there is a 'work_queue_' already, some other thread will take care. 59 if (work_queue_ == nullptr) { 60 work_queue_ = common::make_unique<std::deque<std::function<void()>>>(); 61 HandleWorkQueue(); 62 } 63 } 64 }
可以看到對本類中ComputeConstraint、ComputeConstraintsForOldNodes、HandleWorkQueue方法的呼叫。這裡有個pose_graph::ComputeSubmapPose方法在cartographer/mapping_2d/pose_graph/constraint_builder.h檔案中。子圖submap位姿?
transform::Rigid2d ComputeSubmapPose(const Submap& submap) { return transform::Project2D(submap.local_pose()); }
注意HandleWorkQueue()方法中呼叫了RunOptimization方法。
1 void PoseGraph::HandleWorkQueue() { 2 constraint_builder_.WhenDone( 3 [this](const pose_graph::ConstraintBuilder::Result& result) { 4 { 5 common::MutexLocker locker(&mutex_); 6 constraints_.insert(constraints_.end(), result.begin(), result.end()); 7 } 8 RunOptimization(); 9 10 common::MutexLocker locker(&mutex_); 11 for (const Constraint& constraint : result) { 12 UpdateTrajectoryConnectivity(constraint); 13 } 14 TrimmingHandle trimming_handle(this); 15 for (auto& trimmer : trimmers_) { 16 trimmer->Trim(&trimming_handle); 17 } 18 trimmers_.erase( 19 std::remove_if( 20 trimmers_.begin(), trimmers_.end(), 21 [](std::unique_ptr<mapping::PoseGraphTrimmer>& trimmer) { 22 return trimmer->IsFinished(); 23 }), 24 trimmers_.end()); 25 26 num_nodes_since_last_loop_closure_ = 0; 27 run_loop_closure_ = false; 28 while (!run_loop_closure_) { 29 if (work_queue_->empty()) { 30 work_queue_.reset(); 31 return; 32 } 33 work_queue_->front()(); 34 work_queue_->pop_front(); 35 } 36 LOG(INFO) << "Remaining work items in queue: " << work_queue_->size(); 37 // We have to optimize again. 38 HandleWorkQueue(); 39 }); 40 }PoseGraph::HandleWorkQueue
檢視PoseGraph::RunOptimization方法,
1 void PoseGraph::RunOptimization() 2 { 3 if (optimization_problem_.submap_data().empty()) 4 { 5 return; 6 } 7 8 // No other thread is accessing the optimization_problem_, constraints_ and 9 // frozen_trajectories_ when executing the Solve. Solve is time consuming, so 10 // not taking the mutex before Solve to avoid blocking foreground processing. 11 optimization_problem_.Solve(constraints_, frozen_trajectories_); 12 common::MutexLocker locker(&mutex_); 13 14 const auto& submap_data = optimization_problem_.submap_data(); 15 const auto& node_data = optimization_problem_.node_data(); 16 for (const int trajectory_id : node_data.trajectory_ids()) 17 { 18 for (const auto& node : node_data.trajectory(trajectory_id)) 19 { 20 auto& mutable_trajectory_node = trajectory_nodes_.at(node.id); 21 mutable_trajectory_node.global_pose = 22 transform::Embed3D(node.data.pose) * 23 transform::Rigid3d::Rotation(mutable_trajectory_node.constant_data->gravity_alignment); 24 } 25 26 // Extrapolate all point cloud poses that were not included in the 27 // 'optimization_problem_' yet. 28 const auto local_to_new_global = 29 ComputeLocalToGlobalTransform(submap_data, trajectory_id); 30 const auto local_to_old_global = 31 ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); 32 const transform::Rigid3d old_global_to_new_global = 33 local_to_new_global * local_to_old_global.inverse(); 34 35 const mapping::NodeId last_optimized_node_id = 36 std::prev(node_data.EndOfTrajectory(trajectory_id))->id; 37 auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id)); 38 for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); ++node_it) 39 { 40 auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id); 41 mutable_trajectory_node.global_pose = 42 old_global_to_new_global * mutable_trajectory_node.global_pose; 43 } 44 } 45 global_submap_poses_ = submap_data; 46 }
檢視Constraint 結構體,該結構體在PoseGraphInterface中定義。
1 // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse 2 // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), 3 // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010. 4 struct Constraint { 5 struct Pose { 6 transform::Rigid3d zbar_ij; 7 double translation_weight; 8 double rotation_weight; 9 }; 10 11 SubmapId submap_id; // 'i' in the paper. 12 NodeId node_id; // 'j' in the paper. 13 14 // Pose of the node 'j' relative to submap 'i'. 15 Pose pose; 16 17 // Differentiates between intra-submap (where node 'j' was inserted into 18 // submap 'i') and inter-submap constraints (where node 'j' was not inserted 19 // into submap 'i'). 20 enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; 21 };
相關推薦
Cartographer原始碼閱讀(5):PoseGraph位姿圖
PoseGraph位姿圖 mapping2D::PoseGraph類的註釋: // Implements the loop closure method called Sparse Pose Adjustment (SPA) from// Konolige, Kurt, et al. "E
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原始碼閱讀(2):Node和MapBuilder物件
上文提到特別注意map_builder_bridge_.AddTrajectory(x,x),檢視其中的程式碼。兩點: 首先是map_builder_.AddTrajectoryBuilder(...),呼叫了map_builder_物件的方法。其次是sensor_bridges_鍵值對的賦值。
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
Cartographer原始碼閱讀(4):Node和MapBuilder物件2
MapBuilder的成員變數sensor::Collator sensor_collator_; 再次閱讀MapBuilder::AddTrajectoryBuilder方法。首先構造了mapping::GlobalTrajectoryBuilder例項,接著作為引數構造了CollatedTraj
Cartographer原始碼閱讀(6):LocalTrajectoryBuilder和PoseExtrapolator
LocalTrajectoryBuilder意思是區域性軌跡的構建,下面的類圖中方法的引數沒有畫進去。 注意其中的三個類:PoseExtrapolator類,RealTimeCorrelativeScanMatcher類和CeresScanMatcher類。 (1)PoseExtrapolator類(
牛客網——華為機試(題5:進位制轉換)(Java)
題目描述: 寫出一個程式,接受一個十六進位制的數值字串,輸出該數值的十進位制字串。(多組同時輸入 ) 輸入描述: 輸入一個十六進位制的數值字串。 輸出描述: 輸出該數值的十進位制字串。 示例1: 輸入: 0xA 輸出: 10 import java.ut
Caffe原始碼解析5:Conv_Layer
Vision_layer裡面主要是包括了一些關於一些視覺上的操作,比如卷積、反捲積、池化等等。這裡的類跟data layer一樣好很多種繼承關係。主要包括了這幾個類,其中CuDNN分別是CUDA版本,這裡先不討論,在這裡先討論ConvolutionLayer BaseConvolutionLayer Co
Spring4.3.12原始碼閱讀系列:1-環境搭建
學習任務 近期想增加部分原始碼閱讀經驗,提高自己在造輪子方面的實力,增長些在設計模式應用方面的編碼能力,以及懷著向大佬們膜拜的心情,開始有計劃地閱讀Spring原始碼 前期準備 以下幾項準備事項,算是基本的日常開發環境,就算沒有,也是動動手很快安
Tensorflow object detection API 原始碼閱讀筆記:RPN
Update: 建議先看從程式設計實現角度學習Faster R-CNN,比較直觀。這裡由於原始碼抽象程度較高,顯得比較混亂。 faster_rcnn_meta_arch.py中這兩個對應知乎文章中RPN包含的3*3和1*1卷積: rpn_box_pred
原始碼閱讀系列:為什麼要閱讀原始碼?
一.為什麼要閱讀程式碼 養成閱讀高品質程式碼的習慣,可以提高編寫程式碼的能力。 電腦科學是一門實踐性很強的學科,很多內容在書本上根本學不到。就拿專案的組織來說,沒有什麼書籍專門論述應該如何組織與管理專案的目錄結構,因為這本身就是一種見仁見智的活動,要
Tensorflow object detection API 原始碼閱讀筆記:架構
在之前的博文中介紹過用tf提供的預訓練模型進行inference,非常簡單。這裡我們深入原始碼,瞭解檢測API的程式碼架構,每個部分的深入閱讀留待後續。 '''構建自己模型的介面是虛基類DetectionModel,具體有5個抽象函式需要實現。 ''' o
Tensorflow object detection API 原始碼閱讀筆記:RFCN
有了前面Faster R-CNN的基礎,RFCN就比較容易了。 """object_detection/meta_architectures/rfcn_meta_arch.py The R-FCN
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,
Ethzasl MSF原始碼閱讀(2):百川匯海
這裡有個感覺,就是百川匯海。即IMU資料和相機的訊息資料都彙集到msf_core進行處理。接上一篇, 1. 檢視IMUHandler_ROS::IMUCallback和IMUHandler_ROS::StateCallback回撥函式。 MUHandler_ROS::IMUCallback,傳入的訊息s
Ethzasl MSF原始碼閱讀(3):MSF_Core和PoseMeasurement
1.MSF_Core的三個函式:ProcessIMU、ProcessExternallyPropagatedState和AddMeasurement MSF_Core維護了狀態佇列和觀測值佇列,這裡需要結合論文思考這個狀態佇列的作用。 ProcessIMU方法: 1 template<
jdk原始碼閱讀二:LinkedList
LinkedList介紹 LinkedList是基於連結串列來實現的List。主要特徵如下: 不僅實現了List介面,還實現了Deque,所以是一個雙端佇列。 允許儲存null物件。 非執行緒安全,如果多執行緒操作它,需要外部枷鎖,或者Collecti