rtabmap: fixed support of fixed memory and fixed dictionary (localization-only mode from an already built database) -> updated rtabmap example with "-l" option for localization mode

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@878 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/Memory.cpp

@@ -435,10 +435,13 @@ bool Memory::update(const Image & image, std::map<std::string, float> & stats)
 	// signature like a parent to the memory tree, otherwise add
 	// it as a child to the similar signature.
 	//============================================================
-	this->rehearsal(signature, stats);
+	if(_incrementalMemory)
-	t=timer.ticks()*1000;
+	{
-	stats.insert(std::pair<std::string, float>(std::string("TimingMem/Rehearsal/ms"), t));
+		this->rehearsal(signature, stats);
-	ULOGGER_DEBUG("time rehearsal=%f ms", t);
+		t=timer.ticks()*1000;
+		stats.insert(std::pair<std::string, float>(std::string("TimingMem/Rehearsal/ms"), t));
+		ULOGGER_DEBUG("time rehearsal=%f ms", t);
+	}
 
 	//============================================================
 	// Transfer the oldest signature of the short-term memory to the working memory
@@ -451,7 +454,7 @@ bool Memory::update(const Image & image, std::map<std::string, float> & stats)
 		++_signaturesAdded;
 	}
 
-	if(!_memoryChanged)
+	if(!_memoryChanged && _incrementalMemory)
 	{
 		_memoryChanged = true;
 	}
@@ -1138,7 +1141,7 @@ int Memory::cleanup(const std::list<int> & ignoredIds)
 	// bad signature
 	if(_lastSignature->isBadSignature() || !_incrementalMemory)
 	{
-		moveToTrash(_lastSignature);
+		moveToTrash(_lastSignature, _incrementalMemory);
 		++signaturesRemoved;
 	}
 
@@ -1359,7 +1362,7 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
 
 		// Remove neighbor links with bad signatures (assuming that is done in cleanup),
 		// the bad signatures are always the last signature added.
-		if(s->isBadSignature())
+		if(s->isBadSignature() || !saveToDatabase)
 		{
 			const std::set<int> & neighbors = s->getNeighbors();
 			for(std::set<int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
@@ -1466,7 +1469,7 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
 	ULOGGER_INFO("old=%d, new=%d", oldId, newId);
 	Signature * oldS = _getSignature(oldId);
 	Signature * newS = _getSignature(newId);
-	if(oldS && newS)
+	if(oldS && newS && _incrementalMemory)
 	{
 		const std::set<int> & oldLoopclosureIds = oldS->getLoopClosureIds();
 		if(oldLoopclosureIds.size() && oldLoopclosureIds.find(newS->id()) != oldLoopclosureIds.end())
@@ -1612,6 +1615,11 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
 		{
 			UERROR("newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
 		}
+		if(oldS && newS && !_incrementalMemory)
+		{
+			// Accept loop closure while we don't add a link
+			return true;
+		}
 	}
 	return false;
 }

corelib/src/VWDictionary.cpp

@@ -123,93 +123,97 @@ void VWDictionary::setIncrementalDictionary(bool incrementalDictionary, const st
 {
 	if(!incrementalDictionary)
 	{
-		if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
+		if(!dictionaryPath.empty())
-		   _visualWords.size() == 0)
 		{
-			std::ifstream file;
+			if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
-			if(!dictionaryPath.empty())
+			   _visualWords.size() == 0)
 			{
+				std::ifstream file;
 				file.open(dictionaryPath.c_str(), std::ifstream::in);
-			}
+				if(file.good())
-			if(file.good())
-			{
-				UDEBUG("Deleting old dictionary and loading the new one from \"%s\"", dictionaryPath.c_str());
-				UTimer timer;
-
-				// first line is the header
-				std::string str;
-				std::list<std::string> strList;
-				std::getline(file, str);
-				strList = uSplitNumChar(str);
-				unsigned int dimension  = 0;
-				for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
 				{
-					if(uIsDigit(iter->at(0)))
+					UDEBUG("Deleting old dictionary and loading the new one from \"%s\"", dictionaryPath.c_str());
+					UTimer timer;
+
+					// first line is the header
+					std::string str;
+					std::list<std::string> strList;
+					std::getline(file, str);
+					strList = uSplitNumChar(str);
+					unsigned int dimension  = 0;
+					for(std::list<std::string>::iterator iter = strList.begin(); iter != strList.end(); ++iter)
 					{
-						dimension = std::atoi(iter->c_str());
+						if(uIsDigit(iter->at(0)))
-						break;
+						{
+							dimension = std::atoi(iter->c_str());
+							break;
+						}
 					}
-				}
 
-				if(dimension == 0 || dimension > 1000)
+					if(dimension == 0 || dimension > 1000)
-				{
+					{
-					UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
+						UERROR("Invalid dictionary file, visual word dimension (%d) is not valid, \"%s\"", dimension, dictionaryPath.c_str());
+					}
+					else
+					{
+						// Process all words
+						while(file.good())
+						{
+							std::getline(file, str);
+							strList = uSplit(str);
+							if(strList.size() == dimension+1)
+							{
+								//first one is the visual word id
+								std::list<std::string>::iterator iter = strList.begin();
+								int id = std::atoi(iter->c_str());
+								std::vector<float> descriptor(dimension);
+								++iter;
+								unsigned int i=0;
+
+								//get descriptor
+								for(;i<dimension && iter != strList.end(); ++i, ++iter)
+								{
+									descriptor[i] = std::atof(iter->c_str());
+								}
+								if(i != dimension)
+								{
+									UERROR("");
+								}
+
+								// laplacian not used
+								VisualWord * vw = new VisualWord(id, &(descriptor[0]), dimension, 0);
+								_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
+							}
+							else
+							{
+								UWARN("Cannot parse line \"%s\"", str.c_str());
+							}
+						}
+						this->update();
+						_incrementalDictionary = false;
+					}
+
+
+					UDEBUG("Time changing dictionary = %fs", timer.ticks());
 				}
 				else
 				{
-					// Process all words
+					UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
-					while(file.good())
-					{
-						std::getline(file, str);
-						strList = uSplit(str);
-						if(strList.size() == dimension+1)
-						{
-							//first one is the visual word id
-							std::list<std::string>::iterator iter = strList.begin();
-							int id = std::atoi(iter->c_str());
-							std::vector<float> descriptor(dimension);
-							++iter;
-							unsigned int i=0;
-
-							//get descriptor
-							for(;i<dimension && iter != strList.end(); ++i, ++iter)
-							{
-								descriptor[i] = std::atof(iter->c_str());
-							}
-							if(i != dimension)
-							{
-								UERROR("");
-							}
-
-							// laplacian not used
-							VisualWord * vw = new VisualWord(id, &(descriptor[0]), dimension, 0);
-							_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
-						}
-						else
-						{
-							UWARN("Cannot parse line \"%s\"", str.c_str());
-						}
-					}
-					this->update();
-					_incrementalDictionary = false;
 				}
-
+				file.close();
-
+			}
-				UDEBUG("Time changing dictionary = %fs", timer.ticks());
+			else if(!_incrementalDictionary)
+			{
+				UDEBUG("Dictionary \"%s\" already loaded...", dictionaryPath.c_str());
 			}
 			else
 			{
-				UERROR("Cannot open dictionary file \"%s\"", dictionaryPath.c_str());
+				UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
 			}
-			file.close();
 		}
-		else if(!_incrementalDictionary)
+		else if(_visualWords.size() == 0)
 		{
-			UDEBUG("Dictionary \"%s\" already loaded...", dictionaryPath.c_str());
+			_incrementalDictionary = false;
-		}
-		else
-		{
-			UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
 		}
 	}
 	else if(incrementalDictionary && !_incrementalDictionary)
@@ -358,7 +362,7 @@ void VWDictionary::update()
 
 void VWDictionary::clear()
 {
-	if(_visualWords.size())
+	if(_visualWords.size() && _incrementalDictionary)
 	{
 		UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", _visualWords.size());
 	}

examples/main.cpp

@@ -25,8 +25,10 @@
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-example \"path\"\n"
+			"rtabmap-example [options] \"path\"\n"
-			"  path   Path to a directory of images\n ");
+			"  path       Path to a directory of images\n "
+			"  Options:"
+			"     -l   localization mode: use already built RTAB-Map database to localize\n ");
 	exit(1);
 }
 
@@ -35,11 +37,28 @@ int main(int argc, char * argv[])
 	//ULogger::setType(ULogger::kTypeConsole);
 	//ULogger::setLevel(ULogger::kDebug);
 
+	std::string path;
+	bool localizationMode = false;
+
 	if(argc < 2)
 	{
 		showUsage();
 	}
-	std::string path = argv[1];
+
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(strcmp(argv[i], "-l") == 0)
+		{
+			localizationMode = true;
+		}
+		else
+		{
+			printf("Unrecognized option \"%s\"\n", argv[i]);
+			showUsage();
+		}
+	}
+
+	path = argv[argc-1];
 
 	// rtabmap::Camera is simply a convenience wrapper of OpenCV cv::VideoCapture and cv::imread
 	rtabmap::CameraImages camera(path);
@@ -66,8 +85,15 @@ int main(int argc, char * argv[])
 	// Or SURF hessian treshold:
 	//   parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFHessianThreshold(), "150"));
 
+	if(localizationMode)
+	{
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpIncrementalDictionary(), "false"));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));
+	}
+
 	// Initialize rtabmap: delete/create database...
-	rtabmap.init(parameters);
+	rtabmap.init(parameters, !localizationMode);
 
 	// Process each image of the directory...
 	printf("\nProcessing images... from directory \"%s\"\n", path.c_str());
@@ -75,10 +101,11 @@ int main(int argc, char * argv[])
 	int countLoopDetected=0;
 	int i=0;
 	cv::Mat img = camera.takeImage();
+	int nextIndex = rtabmap.getLastLocationId()+1;
 	while(!img.empty())
 	{
 		// Process image : Main loop of RTAB-Map
-		rtabmap.process(img);
+		rtabmap.process(img, nextIndex);
 
 		// Check if a loop closure is detected and print some info
 		if(rtabmap.getLoopClosureId())
@@ -91,11 +118,11 @@ int main(int argc, char * argv[])
 			printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f) *LOOP %d->%d*\n",
 					i,
 					rtabmap.getLastProcessTime(),
-					rtabmap.getSTM().size(), // short-term memory
+					(int)rtabmap.getSTM().size(), // short-term memory
-					rtabmap.getWM().size(), // working memory
+					(int)rtabmap.getWM().size(), // working memory
 					rtabmap.getLoopClosureId(),
 					rtabmap.getLcHypValue(),
-					rtabmap.getLastLocationId(),
+					nextIndex,
 					rtabmap.getLoopClosureId());
 		}
 		else
@@ -103,12 +130,14 @@ int main(int argc, char * argv[])
 			printf(" #%d ptime(%fs) STM(%d) WM(%d) hyp(%d) value(%.2f)\n",
 					i,
 					rtabmap.getLastProcessTime(),
-					rtabmap.getSTM().size(), // short-term memory
+					(int)rtabmap.getSTM().size(), // short-term memory
-					rtabmap.getWM().size(), // working memory
+					(int)rtabmap.getWM().size(), // working memory
 					rtabmap.getRetrievedId(), // highest loop closure hypothesis
 					rtabmap.getLcHypValue());
 		}
 
+		++nextIndex;
+
 		//Get next image
 		img = camera.takeImage();
 	}

guilib/src/MainWindow.cpp

@@ -487,7 +487,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			//means it will never used for a loop closure detection
 			if(_preferencesDialog->isImagesKept())
 			{
-				if(stat.loopClosureId()>0 || highestHypothesisId>0)
+				if(stat.loopClosureId()>0 || highestHypothesisId>0 || stat.localLoopClosureId()>0)
 				{
 					// order -> loop closure id -> local loop closure id -> highest loop closure hyp id
 					int id = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
@@ -506,19 +506,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 		int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
 		float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
 		int matchId = 0;
-		if(highestHypothesisId > 0 || stat.localLoopClosureId())
+		if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
 		{
 			bool show = true;
 			if(stat.loopClosureId() > 0)
 			{
-				if(highestHypothesisId != stat.loopClosureId())
+				_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::green));
-				{
-					_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::yellow));
-				}
-				else
-				{
-					_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::green));
-				}
 				_ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
 				if(highestHypothesisIsSaved)
 				{
@@ -531,7 +524,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::yellow));
 				_ui->label_matchId->setText(QString("Local match (%1)").arg(stat.localLoopClosureId()));
-
 			}
 			else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
 			{