RTAB-Map : added localLoopClosureId for VSLAM use...

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

corelib/include/rtabmap/core/Rtabmap.h

@@ -74,7 +74,7 @@ public:
 	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
 	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
-	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;
+	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;// <Id,Margin> including nodeId
 	bool isInSTM(int locationId) const;
 	const Statistics & getStatistics() const;
 

corelib/include/rtabmap/core/Statistics.h

@@ -89,6 +89,7 @@ public:
 	void setExtended(bool extended) {_extended = extended;}
 	void setRefImageId(int refImageId) {_refImageId = refImageId;}
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
+	void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
 	void setRefImage(const cv::Mat & image);
 	void setLoopImage(const cv::Mat & image);
 	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
@@ -102,6 +103,7 @@ public:
 	bool extended() const {return _extended;}
 	int refImageId() const {return _refImageId;}
 	int loopClosureId() const {return _loopClosureId;}
+	int localLoopClosureId() const {return _localLoopClosureId;}
 	const cv::Mat & refImage() const {return _refImage;}
 	const cv::Mat & loopImage() const {return _loopImage;}
 	const std::map<int, int> & weights() const {return _weights;}
@@ -118,6 +120,7 @@ private:
 
 	int _refImageId;
 	int _loopClosureId;
+	int _localLoopClosureId; // Note: used by VSLAM
 
 	// extended data start here...
 	cv::Mat _refImage;

corelib/src/Memory.cpp

@@ -1618,7 +1618,7 @@ bool Memory::addLoopClosureLink(int oldId, int newId)
 
 void Memory::dumpMemory(std::string directory) const
 {
-	ULOGGER_DEBUG("");
+	UINFO("Dumping memory to directory \"%s\"", directory.c_str());
 	this->dumpDictionary((directory+"DumpMemoryWordRef.txt").c_str(), (directory+"DumpMemoryWordDesc.txt").c_str());
 	this->dumpSignatures((directory + "DumpMemorySign.txt").c_str());
 	this->dumpMemoryTree((directory + "DumpMemoryTree.txt").c_str());

corelib/src/Rtabmap.cpp

@@ -1237,6 +1237,7 @@ void Rtabmap::process(const cv::Mat & image, int id)
 
 void Rtabmap::dumpData() const
 {
+	UDEBUG("");
 	if(_memory)
 	{
 		_memory->dumpMemory(this->getWorkingDir());

corelib/src/Statistics.cpp

@@ -33,7 +33,8 @@ const std::map<std::string, float> & Statistics::defaultData()
 Statistics::Statistics() :
 	_extended(0),
 	_refImageId(0),
-	_loopClosureId(0)
+	_loopClosureId(0),
+	_localLoopClosureId(0)
 {
 	_defaultDataInitialized = true;
 }

guilib/src/MainWindow.cpp

@@ -489,7 +489,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				if(stat.loopClosureId()>0 || highestHypothesisId>0)
 				{
-					int id = stat.loopClosureId()>0?stat.loopClosureId():highestHypothesisId;
+					// 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;
 					if(!_imagesMap.contains(id))
 					{
 						QByteArray ba;
@@ -505,12 +506,19 @@ 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)
+		if(highestHypothesisId > 0 || stat.localLoopClosureId())
 		{
 			bool show = true;
 			if(stat.loopClosureId() > 0)
 			{
-				_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::green));
+				if(highestHypothesisId != stat.loopClosureId())
+				{
+					_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)
 				{
@@ -519,6 +527,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				_ui->label_matchId->setText(QString("Match ID = %1").arg(stat.loopClosureId()));
 				matchId = stat.loopClosureId();
 			}
+			else if(stat.localLoopClosureId())
+			{
+				_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::yellow));
+				_ui->label_matchId->setText(QString("Local match (%1)").arg(stat.localLoopClosureId()));
+
+			}
 			else if(rejectedHyp && highestHypothesisValue >= _preferencesDialog->getLoopThr())
 			{
 				show = _preferencesDialog->imageRejectedShown() || _preferencesDialog->imageHighestHypShown();;