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

iSAM2 integration (#1249)

Browse Source 
* isam2 integration

* fonctional iSAM2, added related parameters

* updated default params after testing large-scale enviroment

* proximity search optimization

* boost<1.68 fix

* boost version

* Fixed map not showing in graphview

* iSAM2: supporting MM/localization/multi-session modes
This commit is contained in:
matlabbe
2024-03-27 17:18:02 -07:00
committed by GitHub
parent bfeb487dba
commit cc17ebe92b
10 changed files with 450 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+39 -10
View File
@@ -1094,6 +1094,13 @@ void Rtabmap::resetMemory()
{
UERROR("RTAB-Map is not initialized. No memory to reset...");
}
if(_graphOptimizer)
{
delete _graphOptimizer;
_graphOptimizer = Optimizer::create(_parameters);
}
this->setupLogFiles(true);
}
@@ -1177,6 +1184,7 @@ bool Rtabmap::process(
double timeMemoryUpdate = 0;
double timeNeighborLinkRefining = 0;
double timeProximityByTimeDetection = 0;
double timeProximityBySpaceSearch = 0;
double timeProximityBySpaceVisualDetection = 0;
double timeProximityBySpaceDetection = 0;
double timeCleaningNeighbors = 0;
@@ -2598,22 +2606,39 @@ bool Rtabmap::process(
// 1) compare visually with nearest locations
//
UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius);
std::map<int, float> nearestIds;
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
{
nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
}
else
{
nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
}
std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
std::map<int, Transform> nearestPoses;
std::multimap<int, int> links;
if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
{
// get bidirectional links
for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
{
if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
{
links.insert(std::make_pair(iter->second.from(), iter->second.to()));
links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
}
}
}
for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
{
if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
{
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
{
std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first);
UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
{
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
}
}
else
{
nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
}
}
}
UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
@@ -2645,6 +2670,9 @@ bool Rtabmap::process(
}
UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
timeProximityBySpaceSearch = timer.ticks();
ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch);
float proximityFilteringRadius = _proximityFilteringRadius;
if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
{
@@ -4072,6 +4100,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
statistics_.addStatistic(Statistics::kTimingProximity_by_space_search(), timeProximityBySpaceSearch*1000);
statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);