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

fixed map ids not sent on loop closure detection-only mode

Browse Source
This commit is contained in:
Mathieu Labbe
2014-12-01 15:56:49 -05:00
parent e62b1e2769
commit 72bac39943
1 changed files with 33 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+22
View File
@@ -1535,6 +1535,18 @@ bool Rtabmap::process(const SensorData & data)
statistics_.setLoopClosureTransform(sLoop->getLoopClosureIds().at(signature->id()));
}
if(!_rgbdSlamMode)
{
std::map<int, int> mapIds;
mapIds.insert(std::make_pair(signature->id(), _memory->getMapId(signature->id())));
if(_lcHypothesisId)
{
mapIds.insert(std::make_pair(_lcHypothesisId, _memory->getMapId(_lcHypothesisId)));
}
statistics_.setMapIds(mapIds);
}//else... see finalize statistics below
statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
@@ -2107,6 +2119,8 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
{
UDEBUG("");
if(_memory && _memory->getLastWorkingSignature())
{
if(_rgbdSlamMode)
{
if(optimized)
{
@@ -2117,6 +2131,7 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
}
std::set<int> ids = _memory->getWorkingMem(); // STM + WM
@@ -2157,6 +2172,8 @@ void Rtabmap::getGraph(
bool global)
{
if(_memory && _memory->getLastWorkingSignature())
{
if(_rgbdSlamMode)
{
if(optimized)
{
@@ -2167,6 +2184,11 @@ void Rtabmap::getGraph(
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
}
else
{
UWARN("Getting graph while not in RGB-D SLAM mode makes no sense... only map ids are set.");
}
std::set<int> ids = _memory->getWorkingMem(); // STM + WM
ids.insert(_memory->getStMem().begin(), _memory->getStMem().end());