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

Fixed bug where all global loop closures were rejected when LoopRatio was set

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-01 15:51:04 -05:00
parent cf69f17925
commit d3a2c62b45
1 changed files with 7 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+7 -11
View File
@@ -750,7 +750,8 @@ bool Rtabmap::process(const SensorData & data)
const Signature * signature = 0; const Signature * signature = 0;
const Signature * sLoop = 0; const Signature * sLoop = 0;
_loopClosureHypothesis.first = 0; // Don't reset now the last loop closure value _loopClosureHypothesis = std::make_pair(0,0.0f);
std::pair<int, float> lastHighestHypothesis = _highestHypothesis;
_highestHypothesis = std::make_pair(0,0.0f); _highestHypothesis = std::make_pair(0,0.0f);
std::set<int> immunizedLocations; std::set<int> immunizedLocations;
@@ -1051,14 +1052,14 @@ bool Rtabmap::process(const SensorData & data)
{ {
UWARN("rejected hypothesis: by epipolar geometry"); UWARN("rejected hypothesis: by epipolar geometry");
} }
else if(_loopRatio > 0.0f && _loopClosureHypothesis.second && _highestHypothesis.second < _loopRatio*_loopClosureHypothesis.second) else if(_loopRatio > 0.0f && lastHighestHypothesis.second && _highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
{ {
UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)", UWARN("rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
_highestHypothesis.second, _loopRatio, _loopClosureHypothesis.second); _highestHypothesis.second, _loopRatio, lastHighestHypothesis.second);
} }
else if(_loopRatio > 0.0f && _loopClosureHypothesis.second == 0) else if(_loopRatio > 0.0f && lastHighestHypothesis.second == 0)
{ {
UDEBUG("rejected hypothesis: last closure hypothesis is null (loop ratio is on)"); UWARN("rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
} }
else else
{ {
@@ -1069,7 +1070,7 @@ bool Rtabmap::process(const SensorData & data)
timeHypothesesValidation = timer.ticks(); timeHypothesesValidation = timer.ticks();
ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation); ULOGGER_INFO("timeHypothesesValidation=%fs",timeHypothesesValidation);
} }
else if(_highestHypothesis.second < _loopRatio*_loopClosureHypothesis.second) else if(_highestHypothesis.second < _loopRatio*lastHighestHypothesis.second)
{ {
// Used for Precision-Recall computation. // Used for Precision-Recall computation.
// When analysing logs, it's convenient to know // When analysing logs, it's convenient to know
@@ -1079,11 +1080,6 @@ bool Rtabmap::process(const SensorData & data)
//for statistic... //for statistic...
hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0; hypothesisRatio = _loopClosureHypothesis.second>0?_highestHypothesis.second/_loopClosureHypothesis.second:0;
}
if(_loopClosureHypothesis.first == 0)
{
_loopClosureHypothesis.second = 0.0f; // reset loop closure value
} }
} // if(_memory->getWorkingMemSize()) } // if(_memory->getWorkingMemSize())
}// !isBadSignature }// !isBadSignature