From 317d382a245a7237c3a8738633157f91e4fb49c0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 1 Oct 2014 19:52:46 +0000 Subject: [PATCH] New parameter: Rtabmap/StartNewMapOnLoopClosure, when activated and creating a new map, the nodes of the new map will be ignored until a loop closure is found. git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1812 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- corelib/include/rtabmap/core/Parameters.h | 1 + corelib/include/rtabmap/core/Rtabmap.h | 1 + corelib/src/Memory.cpp | 3 +- corelib/src/Rtabmap.cpp | 122 +++++++++++++--------- guilib/src/PreferencesDialog.cpp | 1 + guilib/src/ui/preferencesDialog.ui | 26 ++++- 6 files changed, 97 insertions(+), 57 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 96c8f153..25065cc3 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -143,6 +143,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2, "Maximum locations retrieved at the same time from LTM."); RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration."); RTABMAP_PARAM(Rtabmap, StatisticLogged, bool, false, "Logging enabled."); + RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure, bool, false, "Start a new map only if there is a global loop closure with a previous map.") // Hypotheses selection RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.11, "Loop closing threshold."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 73bf976b..6b55529d 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -173,6 +173,7 @@ private: float _reextractNNDR; int _reextractFeatureType; int _reextractMaxWords; + bool _startNewMapOnLoopClosure; int _lcHypothesisId; float _lcHypothesisValue; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index b3c1a931..a5e9109a 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -1262,7 +1262,7 @@ std::list Memory::cleanup(const std::list & ignoredIds) std::list signaturesRemoved; // bad signature - if((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory) + if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory)) { if(_lastSignature->isBadSignature()) { @@ -2373,7 +2373,6 @@ bool Memory::rehearsalMerge(int oldId, int newId) UDEBUG("Rehearsal merge %d and %d", oldS->id(), newS->id()); - // During rehearsal in STM if(_idUpdatedToNewOneRehearsal) { // update weight diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 6f3ee35f..5229a688 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -105,6 +105,7 @@ Rtabmap::Rtabmap() : _reextractNNDR(Parameters::defaultLccReextractNNDR()), _reextractFeatureType(Parameters::defaultLccReextractFeatureType()), _reextractMaxWords(Parameters::defaultLccReextractMaxWords()), + _startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()), _lcHypothesisId(0), _lcHypothesisValue(0), _retrievedId(0), @@ -359,6 +360,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR); Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType); Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords); + Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); // RGB-D SLAM stuff if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end()) @@ -1681,6 +1683,19 @@ bool Rtabmap::process(const SensorData & data) ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation); } + // If this option activated, add new nodes only if there are linked with a previous map. + // Used when rtabmap is first started, it will wait a + // global loop closure detection before starting the new map, + // otherwise it deletes the current node. + if(_startNewMapOnLoopClosure && + _memory->isIncremental() && // only in mapping mode + signature->getChildLoopClosureIds().size() == 0 && // no loop closure + signature->getNeighbors().size() == 0 && // no neighbors, alone in the current map + _memory->getWorkingMem().size()>1) // The working memory should not be empty + { + _memory->deleteLocation(signature->id()); + } + // Pass this point signature should not be used, since it could be transferred... signature = 0; @@ -1751,64 +1766,67 @@ bool Rtabmap::process(const SensorData & data) // Log info... // TODO : use a specific class which will handle the RtabmapEvent - std::string logF = uFormat("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", - totalTime, - timeMemoryUpdate, - timeReactivations, - timeLikelihoodCalculation, - timePosteriorCalculation, - timeHypothesesCreation, - timeHypothesesValidation, - timeRealTimeLimitReachedProcess, - timeStatsCreation, - _lcHypothesisValue, - 0.0f, - 0.0f, - 0.0f, - 0.0f, - 0.0f, - vpHypothesis, - timeJoiningTrash, - rehearsalValue, - timeEmptyingTrash, - timeRetrievalDbAccess, - timeAddLoopClosureLink); - std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", - _lcHypothesisId, - hypothesis.first, - (int)signaturesRemoved.size(), - 0, - refWordsCount, - dictionarySize, - int(_memory->getWorkingMem().size()), - rejectedHypothesis?1:0, - 0, - 0, - int(signaturesRetrieved.size()), - lcHypothesisReactivated, - refUniqueWordsCount, - _retrievedId, - 0.0f, - rehearsalMaxId, - rehearsalMaxId>0?1:0); - if(_statisticLogsBufferedInRAM) + if(_foutFloat && _foutInt) { - _bufferedLogsF.push_back(logF); - _bufferedLogsI.push_back(logI); - } - else - { - if(_foutFloat) + std::string logF = uFormat("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", + totalTime, + timeMemoryUpdate, + timeReactivations, + timeLikelihoodCalculation, + timePosteriorCalculation, + timeHypothesesCreation, + timeHypothesesValidation, + timeRealTimeLimitReachedProcess, + timeStatsCreation, + _lcHypothesisValue, + 0.0f, + 0.0f, + 0.0f, + 0.0f, + 0.0f, + vpHypothesis, + timeJoiningTrash, + rehearsalValue, + timeEmptyingTrash, + timeRetrievalDbAccess, + timeAddLoopClosureLink); + std::string logI = uFormat("%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", + _lcHypothesisId, + hypothesis.first, + (int)signaturesRemoved.size(), + 0, + refWordsCount, + dictionarySize, + int(_memory->getWorkingMem().size()), + rejectedHypothesis?1:0, + 0, + 0, + int(signaturesRetrieved.size()), + lcHypothesisReactivated, + refUniqueWordsCount, + _retrievedId, + 0.0f, + rehearsalMaxId, + rehearsalMaxId>0?1:0); + if(_statisticLogsBufferedInRAM) { - fprintf(_foutFloat, "%s", logF.c_str()); + _bufferedLogsF.push_back(logF); + _bufferedLogsI.push_back(logI); } - if(_foutInt) + else { - fprintf(_foutInt, "%s", logI.c_str()); + if(_foutFloat) + { + fprintf(_foutFloat, "%s", logF.c_str()); + } + if(_foutInt) + { + fprintf(_foutInt, "%s", logI.c_str()); + } } + UINFO("Time logging = %f...", timer.ticks()); + //ULogger::flush(); } - UINFO("Time logging = %f...", timer.ticks()); - //ULogger::flush(); return true; } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index cb204d11..0aa769e1 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -296,6 +296,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->general_doubleSpinBox_detectionRate->setObjectName(Parameters::kRtabmapDetectionRate().c_str()); _ui->general_spinBox_imagesBufferSize->setObjectName(Parameters::kRtabmapImageBufferSize().c_str()); _ui->general_spinBox_maxRetrieved->setObjectName(Parameters::kRtabmapMaxRetrieved().c_str()); + _ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str()); _ui->lineEdit_databasePath->setObjectName(Parameters::kRtabmapDatabasePath().c_str()); _ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str()); connect(_ui->toolButton_databasePath, SIGNAL(clicked()), this, SLOT(changeDatabasePath())); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index c885bfd9..156b53ad 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -293 + 0 744 - 978 + 900 @@ -86,7 +86,7 @@ QFrame::Raised - 3 + 5 @@ -2222,6 +2222,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + true + + + + + + + Start a new map only if there is a global loop closure detected first with a previous map. If there is no map in memory, a new map is still created. + + + true + + +