i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 10:37:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use BFS instead of A* for proximity graph-depth filtering (#1756)

Browse Source 
* Use BFS instead of A* for proximity graph-depth filtering

* refactored name of the function, added performance test comparison

---------

Co-authored-by: matlabbe <[email protected]>
This commit is contained in:
Torjus Iveland
2026-08-29 22:41:13 -07:00
committed by GitHub
co-authored by matlabbe
parent 8732a2cdc2
commit 9279ab68ca
5 changed files with 394 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Graph.cpp
+33
View File
@@ -1904,6 +1904,39 @@ std::list<std::pair<int, Transform> > computePath(
return path;
}
std::map<int, int> computePathDepths(
const std::multimap<int, int> & links,
int from,
int maxDepth)
{
std::map<int, int> pathDepths;
pathDepths.insert(std::make_pair(from, 0));
std::list<int> frontier;
frontier.push_back(from);
while(!frontier.empty())
{
int currentId = frontier.front();
frontier.pop_front();
int currentDepth = pathDepths.at(currentId);
if(maxDepth > 0 && currentDepth >= maxDepth)
{
continue;
}
for(std::multimap<int, int>::const_iterator iter = links.find(currentId);
iter!=links.end() && iter->first == currentId;
++iter)
{
int nextId = iter->second;
if(pathDepths.find(nextId) == pathDepths.end())
{
pathDepths.insert(std::make_pair(nextId, currentDepth+1));
frontier.push_back(nextId);
}
}
}
return pathDepths;
}
// Dijksta
std::list<int> computePath(
const std::multimap<int, Link> & links,