hdl_graph_slam演算法中的回環檢測部分主要是在loop_detector.hpp**中定義並實現的，主要分為兩部分，分別是：從歷史關鍵幀中尋找可能的回環、對可能的回環進行點雲配準確定回環。因此，整體的**結構並不複雜：
std::vector
detect
(const std::vector<:ptr>
& keyframes,
const std::deque<:ptr>
& new_keyframes, hdl_graph_slam::graphslam& graph_slam)
}return deteted_loops;
}
1k_1
k1，可能1s後又有乙個關鍵幀k
2k_2
k2，關鍵幀k
1k_1
k1和k
2k_2
k2的點雲可能比較相似，這一標準可以避免將k
1k_1
k1和k
2k_2
k2誤檢測為關鍵幀。第二個準則是兩個關鍵幀之間估計的位移小於某乙個閾值，也就是對應著紫色圓圈了。這一準則可以減少需要進行配準的關鍵幀數量，從而加快計算。話不多說，繼續看原始碼吧。
//尋找可能的回環(candidates)
std::vector<:ptr>
find_candidates
(const std::vector<:ptr>
& keyframes,
const keyframe::ptr& new_keyframe)
const
std::vector<:ptr> candidates;
candidates.
reserve(32
);for(
const
auto
& k : keyframes)
const
auto
& pos1 = k-
>node-
>
estimate()
.translation()
;const
auto
& pos2 = new_keyframe-
>node-
>
estimate()
.translation()
;// 準則2:關鍵幀之間位姿估計的位移小於某個閾值
double dist =
(pos1.head<
2>()
- pos2.head<
2>()
).norm()
;if(dist > distance_thresh)
candidates.
push_back
(k);
}return candidates;
}
//點雲配准以尋找正確的回環
loop::ptr matching
(const std::vector<:ptr>
& candidate_keyframes,
const keyframe::ptr& new_keyframe, hdl_graph_slam::graphslam& graph_slam)
registration-
>
setinputtarget
(new_keyframe-
>cloud)
;//pcl中點雲配準演算法,設定目標點雲
double best_score = std::numeric_limits<
double
>
::max()
; keyframe::ptr best_matched;
eigen::matrix4f relative_pose;
pcl::pointcloud
::ptr aligned
(new pcl::pointcloud()
);//配準並得到兩關鍵幀之間的相對位姿
for(
const
auto
& candidate : candidate_keyframes)
best_score = score;
best_matched = candidate;
relative_pose = registration-
>
getfinaltransformation()
;}if(best_score > fitness_score_thresh)
last_edge_accum_distance = new_keyframe-
>accum_distance;
return std::make_shared
(new_keyframe, best_matched, relative_pose)
;}
《原始碼閱讀》原始碼閱讀技巧,原始碼閱讀工具
檢視某個類的完整繼承關係 選中類的名稱，然後按f4 quick type hierarchy quick type hierarchy可以顯示出類的繼承結構，包括它的父類和子類 supertype hierarchy supertype hierarchy可以顯示出類的繼承和實現結構，包括它的父類和...
Cartographer原始碼篇 原始碼分析 1
在安裝編譯cartographer 1.0.0的時候，我們可以看到 主要包括cartorgarpher ros cartographer ceres sover三個部分。其中，ceres solver用於非線性優化，求解最小二乘問題 cartographer ros為ros平台的封裝，獲取感測器資料...
python原始碼剖析 Python原始碼剖析
第頁共 頁python 原始碼剖析 物件機制 1.物件 在python 的世界中，一切都是物件，乙個整數是乙個物件，乙個字串也是 乙個物件，更為奇妙的是，型別也是乙個物件，整數型別是乙個物件，字串類 型也是乙個物件。從 年guido 在那個聖誕節揭開 python 世界的大幕開始，一直到現在，pyt...