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
2026-09-10 05:20:19 +08:00 · 2014-10-01 19:52:46 +00:00
parent 9559eee614
commit 317d382a24
6 changed files with 97 additions and 57 deletions
@@ -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.");
@@ -173,6 +173,7 @@ private:
 	float _reextractNNDR;
 	int _reextractFeatureType;
 	int _reextractMaxWords;
+	bool _startNewMapOnLoopClosure;

 	int _lcHypothesisId;
 	float _lcHypothesisValue;
@@ -1262,7 +1262,7 @@ std::list<int> Memory::cleanup(const std::list<int> & ignoredIds)
 	std::list<int> 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
@@ -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;
 }
@@ -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()));
@@ -63,9 +63,9 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>-293</y>
+            <y>0</y>
            <width>744</width>
-            <height>978</height>
+            <height>900</height>
           </rect>
          </property>
          <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>3</number>
+                <number>5</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2222,6 +2222,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
+                      <item row="3" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_startNewMapOnLoopClosure">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_185">
+                        <property name="text">
+                         <string>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.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                     </layout>
                    </item>
                    <item>