i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 01:57:45 +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,
corelib/src/Rtabmap.cpp
+9 -6
View File
@@ -2746,7 +2746,6 @@ bool Rtabmap::process(
std::map<int, Transform> nearestPoses;
std::map<int, Transform> optimizedPosesWithOdomCache;
std::multimap<int, int> links;
std::map<int, Transform> * refPoses = &_optimizedPoses;
if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
{
// get bidirectional links
@@ -2765,7 +2764,6 @@ bool Rtabmap::process(
// mapping mode while being localized on the previous session.
optimizedPosesWithOdomCache = _optimizedPoses;
optimizedPosesWithOdomCache.insert(_odomCachePoses.begin(), _odomCachePoses.end());
refPoses = &optimizedPosesWithOdomCache;
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end(); ++iter)
{
if(uContains(optimizedPosesWithOdomCache, iter->second.from()) &&
@@ -2778,18 +2776,23 @@ bool Rtabmap::process(
}
}
}
std::map<int, int> proximityPathDepths;
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
{
proximityPathDepths = graph::computePathDepths(links, signature->id(), _proximityMaxGraphDepth);
}
for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
{
if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
{
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
{
std::list<std::pair<int, Transform> > path = graph::computePath(*refPoses, links, signature->id(), iter->first);
UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
std::map<int, int>::const_iterator depthIter = proximityPathDepths.find(iter->first);
if(depthIter == proximityPathDepths.end())
{
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
continue;
}
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
}
else
{