i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed #1300 (proximity sort by distance)

Browse Source
This commit is contained in:
matlabbe
2024-06-16 11:11:39 -07:00
parent 0d221a806f
commit 7c601bb6e8
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Rtabmap.cpp
View File

@@ -2666,7 +2666,7 @@ bool Rtabmap::process(
UDEBUG("got %d paths", (int)nearestPathsNotSorted.size()); UDEBUG("got %d paths", (int)nearestPathsNotSorted.size());
// sort nearest paths by highest likelihood (if two have same likelihood, sort by id) // sort nearest paths by highest likelihood (if two have same likelihood, sort by id)
std::map<NearestPathKey, std::map<int, Transform> > nearestPaths; std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
Transform currentPoseInv = _optimizedPoses.at(signature->id()); Transform currentPoseInv = _optimizedPoses.at(signature->id()).inverse();
for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter) for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
{ {
const std::map<int, Transform> & path = iter->second; const std::map<int, Transform> & path = iter->second;