-Refactored HypVerification classes (added verification by signatures' similarity), there is no more complicated rejectedHypReason, now it's only true if valide or false if verification fails...

git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@86 f169173b-cf89-36c8-b27e-44dbe73f0c83

app/src/main.cpp

@@ -49,6 +49,8 @@ int main(int argc, char* argv[])
 	/* Start thread's task */
 	mainWindow->showNormal();
 
+	rtabmap->start();
+
 	// Now wait for application to finish
 	app->connect( app, SIGNAL( lastWindowClosed() ),
 				app, SLOT( quit() ) );

corelib/include/rtabmap/core/Parameters.h

@@ -121,7 +121,7 @@ typedef std::pair<const std::string, std::string> ParametersPair;
 class RTABMAP_EXP Parameters
 {
 	// Rtabmap parameters
-	RTABMAP_PARAM(Rtabmap, VhStrategy, 	        int, 0);	   // Simple 0, Epipolar 1
+	RTABMAP_PARAM(Rtabmap, VhStrategy, 	        int, 0);	   // None 0, Similarity 1, Epipolar 2
 	RTABMAP_PARAM(Rtabmap, PublishStats, 	     bool, true); // Publishing statistics
 	RTABMAP_PARAM(Rtabmap, ReactivationThr,     float, 0.0); // Reactivation threshold
 	RTABMAP_PARAM(Rtabmap, TimeThr, 		     float, 0.7); // Maximum time allowed for the detector (s) (0 means infinity)
@@ -197,6 +197,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.24 0.18 0.1 0.04 0.01"); // Prediction of loop closures (Gaussian-like, must be pair size) - Format: {VirtualPlaceProb, LoopClosureProb, BackwardNeighborLvl1, ForwardNeighborLvl1, BackwardNeighborLvl2, ForwardNeighborLvl2, ...}
 
 	// Verify hypotheses
+	RTABMAP_PARAM(Vh,   Similarity,    float, 0.5); // Minimum similarity to accept an hypothesis
 	RTABMAP_PARAM(VhEp, MatchCountMin, int, 8);     // Minimum of matching visual words pairs to accept the loop hypothesis
 	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3.0);  // Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier
 	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99); // Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC

corelib/include/rtabmap/core/Rtabmap.h

@@ -39,7 +39,7 @@ namespace rtabmap
 
 class Signature;
 
-class VerifyHypotheses;
+class HypVerificator;
 class Memory;
 class BayesFilter;
 class SMState;
@@ -60,7 +60,7 @@ public:
 		kStateDeletingMemory
 	};
 
-	enum VhStrategy {kVhSimple, kVhEpipolar, kVhUndef};
+	enum VhStrategy {kVhNone, kVhSim, kVhEpipolar, kVhUndef};
 
 	static const char * kDefaultIniFileName;
 	static const char * kDefaultIniFilePath;
@@ -153,7 +153,7 @@ private:
 
 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
-	VerifyHypotheses * _vhStrategy;
+	HypVerificator * _vhStrategy;
 	BayesFilter * _bayesFilter;
 
 	Memory * _memory;

corelib/include/rtabmap/core/RtabmapEvent.h

@@ -46,7 +46,7 @@ namespace rtabmap
 class RTABMAP_EXP Statistics
 {
 	RTABMAP_STATS(Loop, Closure_id,);
-	RTABMAP_STATS(Loop, Rejected_reason,);
+	RTABMAP_STATS(Loop, RejectedHypothesis,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
 	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
 	RTABMAP_STATS(Loop, Vp_likelihood,);

corelib/src/KeypointMemory.cpp

@@ -419,7 +419,7 @@ void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrate
 				std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
 				std::list<int> pairsId;
 				std::set<int> pairsIdSet;
-				VerifyHypothesesEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
+				HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
 				std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator kpIt = pairs.begin();
 				std::list<int>::iterator idIt = pairsId.begin();
 				for(; kpIt != pairs.end() && idIt != pairsId.end(); ++kpIt, ++idIt)
@@ -570,7 +570,7 @@ void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrate
 				{
 					std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
 					std::list<int> pairsId;
-					VerifyHypothesesEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
+					HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
 					std::set<int> ignoredIds(pairsId.begin(), pairsId.end());
 					int dif = maxWords - (int)wordsA.size();
 					newWords = getMostDescriptiveWords(wordsB, dif, ignoredIds);
@@ -598,7 +598,7 @@ void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrate
 			    	}
 			    	std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
 					std::list<int> pairsId;
-					VerifyHypothesesEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
+					HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
 					std::set<int> ignoredIds(pairsId.begin(), pairsId.end());
 					int dif = maxWords - (int)wordsB.size();
 					newWords = getMostDescriptiveWords(wordsA, dif, ignoredIds);

corelib/src/Rtabmap.cpp

@@ -299,8 +299,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		vhStrategy = (VhStrategy)std::atoi((*iter).second.c_str());
 	}
 	if(!_vhStrategy ||
-	   (vhStrategy!=kVhUndef && ( (vhStrategy == kVhSimple && !dynamic_cast<VerifyHypothesesSimple*>(_vhStrategy)) ||
+	   (vhStrategy!=kVhUndef && ( (vhStrategy == kVhSim && !dynamic_cast<HypVerificatorSim*>(_vhStrategy)) ||
-							      (vhStrategy == kVhEpipolar && !dynamic_cast<VerifyHypothesesEpipolarGeo*>(_vhStrategy)) ) ))
+							      (vhStrategy == kVhEpipolar && !dynamic_cast<HypVerificatorEpipolarGeo*>(_vhStrategy)) ) ))
 	{
 		if(_vhStrategy)
 		{
@@ -310,11 +310,13 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		switch(vhStrategy)
 		{
 		case kVhEpipolar:
-			_vhStrategy = new VerifyHypothesesEpipolarGeo(parameters);
+			_vhStrategy = new HypVerificatorEpipolarGeo(parameters);
+			break;
+		case kVhSim:
+			_vhStrategy = new HypVerificatorSim(parameters);
 			break;
-		case kVhSimple:
 		default:
-			_vhStrategy = new VerifyHypothesesSimple(parameters);
+			_vhStrategy = new HypVerificator(parameters);
 			break;
 		}
 	}
@@ -605,6 +607,7 @@ void Rtabmap::process()
 
 	int refId = Memory::kIdInvalid;
 	float hypothesisRatio = 0.0f; // Only used for statistics
+	bool rejectedHypothesis = false;
 
 	std::map<int, float> likelihood;
 	std::map<int, float> adjustedLikelihood;
@@ -613,7 +616,6 @@ void Rtabmap::process()
 	std::list<std::pair<int, float> > reactivateHypotheses;
 
 	std::map<int, int> childCount;
-	int rejectLoopReason = 0;
 	unsigned int signaturesReactivated = 0;
 
 	const Signature * signature = 0;
@@ -819,71 +821,49 @@ void Rtabmap::process()
 			timeHypothesesCreation = timer.ticks();
 			ULOGGER_INFO("timeHypothesesCreation=%f",timeHypothesesCreation);
 
-			rejectLoopReason = 1; //Default: NOT_ENOUGH_HYPOTHESES
 			if(hypotheses.size())
 			{
 				// Loop closure Threshold
-				if(!signature->isBadSignature() &&
+				if(likelihood.at(hypotheses.front().first) > 0.0f &&
-				   likelihood.at(hypotheses.front().first) > 0.0f &&
+				   hypotheses.front().second >= _loopThr)
-				   hypotheses.front().second >= _loopThr &&
-				   hypotheses.front().second >= _loopRatio*_highestHypothesisValue &&
-				   _highestHypothesisValue)
 				{
 					//============================================================
 					// Hypothesis verification for loop closure with geometric
 					// information (like the epipolar geometry or using the local map
 					// associated with the signature)
 					//============================================================
-					//std::list<int> h;
+					if(hypotheses.front().second >= _loopRatio*_highestHypothesisValue &&
-					//h.push_back(hypotheses.front().first);
+					   _highestHypothesisValue &&
-					_lcHypothesisId = hypotheses.front().first;
+					   _vhStrategy->verify(signature, _memory->getSignature(hypotheses.front().first)))
-					//_lcHypothesisId = _vhStrategy->verifyHypotheses(h, _memory);
+					{
+						_lcHypothesisId = hypotheses.front().first;
+					}
+					else
+					{
+						rejectedHypothesis = true;
+					}
 
 					timeHypothesesValidation = timer.ticks();
 					ULOGGER_INFO("timeHypothesesValidation=%f",timeHypothesesValidation);
 
-					//rejectLoopReason = 10 + _vhStrategy->getStatus();
-
 					// We are tracking the next loop closures,
 					// reset the retrieval margin
 					_spreadMargin = 0;
-
-					rejectLoopReason = 10; //Accepted
-
-				}
-
-				if(hypotheses.front().second < _loopThr)
-				{
-					rejectLoopReason = 2;
 				}
 
+				//============================================================
+				// Retrieval id update
+				//============================================================
 				int lastReactivatedId = _reactivateId;
-				// Recalling Threshold
+				std::list<std::pair<int, float> > hyp;
-				//this->selectHypotheses(posterior, reactivateHypotheses, false);
+				this->selectHypotheses(posterior, hyp, false);
-				//if(reactivateHypotheses.front().second >= _remThr)
+				if(likelihood.at(hyp.front().first) > _remThr)
-				//{
+				{
-					//Priority to single hypotheses over _remThr
+					_reactivateId = hyp.front().first;
-				//	_reactivateId = reactivateHypotheses.front().first;
+				}
-				//}
-				//else
-				//if(_lcHypothesisId)
-				//{
-				//	_reactivateId = _lcHypothesisId;
-				//}
-				//else
-				//{
-					//_reactivateId = hypotheses.front().first;
-					std::list<std::pair<int, float> > hyp;
-					this->selectHypotheses(posterior, hyp, false);
-					if(likelihood.at(hyp.front().first) > _remThr)
-					{
-						_reactivateId = hyp.front().first;
-					}
-				//}
 
 				if(hypotheses.front().second < _loopRatio*_highestHypothesisValue)
 				{
-					rejectLoopReason = 3;
 					if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hypotheses.front().first) != reactivatedIds.end())
 					{
 						// We are loosing the next loop closures (it
@@ -957,7 +937,7 @@ void Rtabmap::process()
 	}
 
 	// only send actions if rejectLoopReason!=3 (decreasing hypotheses)
-	if(sLoop && (_actionsSentRejectHyp || rejectLoopReason != 3))
+	if(sLoop && (_actionsSentRejectHyp || !rejectedHypothesis))
 	{
 		// select the actions of the neighbor with the highest
 		// weight (select the more recent if some have the same weight)
@@ -1115,7 +1095,7 @@ void Rtabmap::process()
 			}
 
 			//Epipolar geometry constraint
-			stat->addStatistic(Statistics::kLoopRejected_reason(), rejectLoopReason);
+			stat->addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
 			timeStatsCreation = timer.ticks();
 		}
 		else
@@ -1245,7 +1225,7 @@ void Rtabmap::process()
 				refWordsCount,
 				dictionarySize,
 				int(_memory->getWorkingMemSize()),
-				rejectLoopReason,
+				rejectedHypothesis?1:0,
 				processMemoryUsed,
 				databaseMemoryUsed,
 				signaturesReactivated,

corelib/src/Signature.cpp

@@ -188,7 +188,7 @@ float KeypointSignature::compareTo(const Signature * s) const
 			std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
 			std::list<int> pairsId;
 			int totalWords = _words.size()>words.size()?_words.size():words.size();
-			VerifyHypothesesEpipolarGeo::findPairsDirect(words, _words, pairs, pairsId);
+			HypVerificatorEpipolarGeo::findPairsDirect(words, _words, pairs, pairsId);
 
 			similarity = float(pairs.size()) / float(totalWords);
 

corelib/src/VerifyHypotheses.cpp

@@ -20,7 +20,6 @@
 #include "VerifyHypotheses.h"
 #include "rtabmap/core/Parameters.h"
 #include "Signature.h"
-#include "Memory.h"
 #include <cstdlib>
 #include <opencv2/calib3d/calib3d.hpp>
 
@@ -29,139 +28,65 @@
 namespace rtabmap
 {
 
-VerifyHypotheses::VerifyHypotheses(const ParametersMap & parameters) :
+HypVerificator::HypVerificator(const ParametersMap & parameters)
-	_status(0)
 {
 	this->parseParameters(parameters);
 }
 
-void VerifyHypotheses::parseParameters(const ParametersMap & parameters)
+void HypVerificator::parseParameters(const ParametersMap & parameters)
 {
 }
 
+bool HypVerificator::verify(const Signature * ref, const Signature * hyp)
+{
+	UDEBUG("");
+	return ref && hyp && !ref->isBadSignature() && !hyp->isBadSignature();
+}
+
 
 
 /////////////////////////
-// VerifyHypothesesSimple
+// HypVerificatorSim
 /////////////////////////
-VerifyHypothesesSimple::VerifyHypothesesSimple(const ParametersMap & parameters) :
+HypVerificatorSim::HypVerificatorSim(const ParametersMap & parameters) :
-	VerifyHypotheses(parameters)
+	HypVerificator(parameters),
+	_similarity(Parameters::defaultVhSimilarity())
 {
 	this->parseParameters(parameters);
 }
 
-VerifyHypothesesSimple::~VerifyHypothesesSimple()
+HypVerificatorSim::~HypVerificatorSim()
 {
 }
 
-void VerifyHypothesesSimple::parseParameters(const ParametersMap & parameters)
+void HypVerificatorSim::parseParameters(const ParametersMap & parameters)
-{
-	VerifyHypotheses::parseParameters(parameters);
-}
-
-int VerifyHypothesesSimple::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
-{
-	int hypothesis = 0;
-
-	if(!hypotheses.empty())
-	{
-		for(std::list<int>::const_iterator i = hypotheses.begin(); i!=hypotheses.end(); ++i)
-		{
-			if(*i > 0)
-			{
-				hypothesis = *i;
-				break;
-			}
-		}
-	}
-
-	return hypothesis;
-}
-
-
-/////////////////////////
-// VerifyHypothesesSignSeq
-/////////////////////////
-/*VerifyHypothesesSignSeq::VerifyHypothesesSignSeq(const ParametersMap & parameters) :
-	VerifyHypotheses(parameters),
-	_seqLength(Parameters::defaultVhEpSeqLength())
-{
-	this->parseParameters(parameters);
-}
-
-VerifyHypothesesSignSeq::~VerifyHypothesesSignSeq() {
-
-}
-
-void VerifyHypothesesSignSeq::parseParameters(const ParametersMap & parameters)
 {
 	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kVhEpSeqLength())) != parameters.end())
+	if((iter=parameters.find(Parameters::kVhSimilarity())) != parameters.end())
 	{
-		_seqLength = atoi((*iter).second.c_str());
+		_similarity = std::atof((*iter).second.c_str());
 	}
-	VerifyHypotheses::parseParameters(parameters);
+
+	HypVerificator::parseParameters(parameters);
 }
 
-int VerifyHypothesesSignSeq::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
+bool HypVerificatorSim::verify(const Signature * ref, const Signature * hyp)
 {
-	int hypothesis = 0;
+	UDEBUG("");
-	std::map<int, int> hypothesesToKeep;
+	if(ref && hyp)
-
-	// update hypotheses
-	std::map<int, int>::iterator tmp;
-	for(std::list<int>::const_iterator j=hypotheses.begin(); j!=hypotheses.end(); ++j)
 	{
-		// Add it like a new hypothesis
+		return ref->compareTo(hyp) >= _similarity;
-		hypothesesToKeep.insert(std::pair<int, int>(*j, 1)); // NOTE : It will be deleted if an updated hypothesis gives the same id.
-
-		// If we have already this hypothesis, just keep it
-		tmp = _hypotheses.find(*j);
-		if(tmp != _hypotheses.end())
-		{
-			hypothesesToKeep.insert(std::pair<int, int>((*tmp).first, (*tmp).second));
-		}
-
-		// Forward hypothesis
-		tmp = _hypotheses.find(*j-1);
-		if(tmp != _hypotheses.end())
-		{
-			hypothesesToKeep.insert(std::pair<int, int>(*j, (*tmp).second+1));
-		}
-
-		// Backward hypothesis
-		tmp = _hypotheses.find(*j+1);
-		if(tmp != _hypotheses.end())
-		{
-			hypothesesToKeep.insert(std::pair<int, int>(*j, (*tmp).second-1));
-		}
 	}
 
-	_hypotheses = hypothesesToKeep;
+	return false;
-
+}
-	// if an hypothesis has at least 3 references, return the id
-	if(_hypotheses.size()>0)
-	{
-		for(std::map<int, int>::iterator i=_hypotheses.begin(); i!=_hypotheses.end(); ++i)
-		{
-			if((*i).second > _seqLength)
-			{
-				hypothesis = (*i).first;
-				break; // return the first
-			}
-		}
-	}
-
-	return hypothesis;
-}*/
-
 
 
 /////////////////////////
-// VerifyHypothesesEpipolarGeo
+// HypVerificatorEpipolarGeo
 /////////////////////////
-VerifyHypothesesEpipolarGeo::VerifyHypothesesEpipolarGeo(const ParametersMap & parameters) :
+HypVerificatorEpipolarGeo::HypVerificatorEpipolarGeo(const ParametersMap & parameters) :
-	VerifyHypotheses(parameters),
+	HypVerificator(parameters),
 	_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
 	_ransacParam1(Parameters::defaultVhEpRansacParam1()),
 	_ransacParam2(Parameters::defaultVhEpRansacParam2())
@@ -169,11 +94,11 @@ VerifyHypothesesEpipolarGeo::VerifyHypothesesEpipolarGeo(const ParametersMap & p
 	this->parseParameters(parameters);
 }
 
-VerifyHypothesesEpipolarGeo::~VerifyHypothesesEpipolarGeo() {
+HypVerificatorEpipolarGeo::~HypVerificatorEpipolarGeo() {
 
 }
 
-void VerifyHypothesesEpipolarGeo::parseParameters(const ParametersMap & parameters)
+void HypVerificatorEpipolarGeo::parseParameters(const ParametersMap & parameters)
 {
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
@@ -188,66 +113,26 @@ void VerifyHypothesesEpipolarGeo::parseParameters(const ParametersMap & paramete
 	{
 		_ransacParam2 = std::atof((*iter).second.c_str());
 	}
-	VerifyHypotheses::parseParameters(parameters);
+	HypVerificator::parseParameters(parameters);
 }
 
-void VerifyHypothesesEpipolarGeo::setStatus(int status)
+bool HypVerificatorEpipolarGeo::verify(const Signature * ref, const Signature * hyp)
 {
-	// Only set if the status was not set before. This will keep the
+	UDEBUG("");
-	// first error (when comparing with many signatures)
+	const KeypointSignature * ssRef = dynamic_cast<const KeypointSignature *>(ref);
-	if(status == UNDEFINED || this->getStatus() == UNDEFINED || status == ACCEPTED)
+	const KeypointSignature * ssHyp = dynamic_cast<const KeypointSignature *>(hyp);
+	if(ssRef && ssHyp)
 	{
-		VerifyHypotheses::setStatus(status);
+		return doEpipolarGeometry(ssHyp, ssRef);
 	}
+
+	return false;
 }
 
-int VerifyHypothesesEpipolarGeo::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
+bool HypVerificatorEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB)
-{
-	ULOGGER_DEBUG("");
-	int hypothesis = 0;
-	this->setStatus(UNDEFINED);
-
-	if(mem && !hypotheses.empty())
-	{
-		const KeypointSignature * ssRef = dynamic_cast<const KeypointSignature *>(mem->getLastSignature());
-		if(ssRef)
-		{
-			unsigned int i=0;
-			for(std::list<int>::const_iterator iter = hypotheses.begin(); iter!=hypotheses.end(); ++iter)
-			{
-				if(*iter > 0)
-				{
-					const KeypointSignature * ssHyp = dynamic_cast<const KeypointSignature *>(mem->getSignature(*iter));
-					if(ssHyp)
-					{
-						if(doEpipolarGeometry(ssHyp, ssRef))
-						{
-							hypothesis = *iter;
-							break;
-						}
-					}
-				}
-				++i;
-			}
-		}
-	}
-	else if(!mem)
-	{
-		this->setStatus(this->MEMORY_IS_NULL);
-	}
-	else if(hypotheses.empty())
-	{
-		this->setStatus(this->NO_HYPOTHESIS);
-	}
-
-	return hypothesis;
-}
-
-bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB)
 {
 	if(ssA == 0 || ssB == 0)
 	{
-		this->setStatus(this->NULL_MATCHING_SURF_SIGNATURES);
 		return false;
 	}
 	ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
@@ -280,7 +165,6 @@ bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * s
 			float(pairs.size()) / float(similarities));
 	if(pairsCount < _matchCountMinAccepted)
 	{
-		this->setStatus(this->NOT_ENOUGH_MATCHING_PAIRS);
 		return false;
 	}
 
@@ -452,17 +336,14 @@ bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * s
 
 		if(goodCount < _matchCountMinAccepted)
 		{
-			this->setStatus(this->EPIPOLAR_CONSTRAINT_FAILED);
 			ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d), min is %d", goodCount, _matchCountMinAccepted);
 			return false;
 		}
 		else
 		{
-			this->setStatus(this->ACCEPTED);
 			return true;
 		}
 	}
-	this->setStatus(this->FUNDAMENTAL_MATRIX_NOT_FOUND);
 	return false;
 }
 
@@ -470,7 +351,7 @@ bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * s
  * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
  * realPairsCount = 5
  */
-int VerifyHypothesesEpipolarGeo::findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA,
+int HypVerificatorEpipolarGeo::findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
 		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
 		std::list<int> & pairsId)
@@ -500,7 +381,7 @@ int VerifyHypothesesEpipolarGeo::findPairsDirect(const std::multimap<int, cv::Ke
  * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
  * realPairsCount = 5
  */
-int VerifyHypothesesEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA,
+int HypVerificatorEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
 		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
 		std::list<int> & pairsId)
@@ -531,7 +412,7 @@ int VerifyHypothesesEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPo
  * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
  * realPairsCount = 5
  */
-int VerifyHypothesesEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
+int HypVerificatorEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
 		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
 		std::list<int> & pairsId)
@@ -565,7 +446,7 @@ int VerifyHypothesesEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPo
  * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [1 2 4 6]
  * return 4
  */
-std::list<int> VerifyHypothesesEpipolarGeo::findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA,
+std::list<int> HypVerificatorEpipolarGeo::findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB)
 {
 	std::list<int> sameIds;
@@ -580,7 +461,7 @@ std::list<int> VerifyHypothesesEpipolarGeo::findSameIds(const std::multimap<int,
 	return sameIds;
 }
 
-int VerifyHypothesesEpipolarGeo::getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB)
+int HypVerificatorEpipolarGeo::getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB)
 {
 	const std::list<int> & ids = uUniqueKeys(wordsA);
 	int total = 0;

corelib/src/VerifyHypotheses.h

@@ -32,85 +32,51 @@
 namespace rtabmap
 {
 
-class Memory;
+class Signature;
 
-class RTABMAP_EXP VerifyHypotheses
+// return always true, i.e, there is no verification
+class RTABMAP_EXP HypVerificator
 {
 public:
-	virtual ~VerifyHypotheses() {}
+	HypVerificator(const ParametersMap & parameters = ParametersMap());
-	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem) = 0;
+	virtual ~HypVerificator() {}
+	virtual bool verify(const Signature * ref, const Signature * hyp);
 
-	int getStatus() {return _status;}
-	virtual void parseParameters(const ParametersMap & parameters);
-
-protected:
-	VerifyHypotheses(const ParametersMap & parameters = ParametersMap());
-	virtual void setStatus(int status) {_status = status;}
-
-private:
-	int _status;
-};
-
-
-/////////////////////////
-// VerifyHypothesesSimple
-/////////////////////////
-class VerifyHypothesesSimple : public VerifyHypotheses {
-public:
-	VerifyHypothesesSimple(const ParametersMap & parameters = ParametersMap());
-	virtual ~VerifyHypothesesSimple();
-	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
 	virtual void parseParameters(const ParametersMap & parameters);
 };
 
 
 /////////////////////////
-// VerifyHypothesesSignSeq
+// HypVerificatorSim
 /////////////////////////
-/*class VerifyHypothesesSignSeq : public VerifyHypotheses {
+class HypVerificatorSim : public HypVerificator {
 public:
-	VerifyHypothesesSignSeq(const ParametersMap & parameters = ParametersMap());
+	HypVerificatorSim(const ParametersMap & parameters = ParametersMap());
-	virtual ~VerifyHypothesesSignSeq();
+	virtual ~HypVerificatorSim();
-	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
+	virtual bool verify(const Signature * ref, const Signature * hyp);
 	virtual void parseParameters(const ParametersMap & parameters);
 
 private:
-	std::map<int, int> _hypotheses;
+	float _similarity;
-	int _seqLength;
+};
-
-};*/
-
 
 
 /////////////////////////
-// VerifyHypothesesEpipolarGeo
+// HypVerificatorEpipolarGeo
 /////////////////////////
 class KeypointSignature;
 
-class RTABMAP_EXP VerifyHypothesesEpipolarGeo : public VerifyHypotheses
+class RTABMAP_EXP HypVerificatorEpipolarGeo : public HypVerificator
 {
-public:
-	enum STATUS
-	{
-		UNDEFINED,
-		ACCEPTED,
-		NO_HYPOTHESIS,
-		MEMORY_IS_NULL,
-		NOT_ENOUGH_MATCHING_PAIRS,
-		EPIPOLAR_CONSTRAINT_FAILED,
-		NULL_MATCHING_SURF_SIGNATURES,
-		FUNDAMENTAL_MATRIX_NOT_FOUND
-	};
-
 public:
 	static int findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
 	static int findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
 	static int findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
 	static std::list<int> findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
 public:
-	VerifyHypothesesEpipolarGeo(const ParametersMap & parameters = ParametersMap());
+	HypVerificatorEpipolarGeo(const ParametersMap & parameters = ParametersMap());
-	virtual ~VerifyHypothesesEpipolarGeo();
+	virtual ~HypVerificatorEpipolarGeo();
-	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
+	virtual bool verify(const Signature * ref, const Signature * hyp);
 	virtual void parseParameters(const ParametersMap & parameters);
 
 	int getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
@@ -123,9 +89,6 @@ public:
 	void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
 	void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
 
-protected:
-	virtual void setStatus(int status);
-
 private:
 	bool doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB);
 

corelib/tests/Tests.cpp

@@ -468,7 +468,7 @@ void Tests::testVerifyHypotheses()
 	wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
 	wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));
 
-	int total = VerifyHypothesesEpipolarGeo::findPairsAll(wordsA, wordsB, pairs, pairsId);
+	int total = HypVerificatorEpipolarGeo::findPairsAll(wordsA, wordsB, pairs, pairsId);
 
 	//printf("[%d,%d]\n", total, (int)pairs.size());
 	CPPUNIT_ASSERT(total == 5);

guilib/src/MainWindow.cpp

@@ -303,9 +303,9 @@ void MainWindow::handleEvent(UEvent* anEvent)
 		RtabmapEvent * rtabmapEvent = (RtabmapEvent*)anEvent;
 		const Statistics & stats = rtabmapEvent->getStats();
 		int highestHypothesisId = int(uValue(stats.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
-		int rejectLoopReason = int(uValue(stats.data(), Statistics::kLoopRejected_reason(), 0.0f));
+		bool rejectedHyp = bool(uValue(stats.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
 		if((stats.loopClosureId() > 0 && _ui->actionPause_on_match->isChecked()) ||
-		   (stats.loopClosureId() == 0 && highestHypothesisId > 0 && _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() && (rejectLoopReason >= 12 || (rejectLoopReason == 3 && uValue(stats.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f)>=_preferencesDialog->getLoopThr()))))
+		   (stats.loopClosureId() == 0 && highestHypothesisId > 0 && _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() && rejectedHyp))
 		{
 			if(_state != kPaused)
 			{
@@ -423,7 +423,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			}
 			ULOGGER_DEBUG("");
 			_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
-			int rejectLoopReason = int(uValue(stat.data(), Statistics::kLoopRejected_reason(), 0.0f));
+			int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
 			if(stat.loopClosureId()>0)
 			{
 				_ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
@@ -453,8 +453,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					_ui->imageView_loopClosure->scene()->addPixmap(QPixmap::fromImage(img))->setVisible(this->_ui->checkBox_showImageLoop->isChecked());
 				}
 			}
-			else if(rejectLoopReason>=12 ||
+			else if(rejectedHyp)
-					(rejectLoopReason == 3 && uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f)>=_preferencesDialog->getLoopThr()))
 			{
 				ULOGGER_DEBUG("");
 				_ui->label_stats_loopClosuresRejected->setText(QString::number(_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
@@ -462,87 +461,37 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 				if(_preferencesDialog->imageRejectedShown())
 				{
-					// rejectLoopReason,
+					QColor color = Qt::yellow;
-					// 1 is for not enough hypotheses, [not supposed to occur if highestHypothesisId is set]
+					QGraphicsTextItem * textItem = _ui->imageView_loopClosure->scene()->addText(tr("Rejected hypothesis"));
-					// 10 to ... means rejecting reasons from the hypotheses verification step.
+
-					// UNDEFINED, 10
+					textItem->setDefaultTextColor(QColor(255-color.red(), 255-color.green(), 255-color.blue())); // color inverted
-					// ACCEPTED, 11
+					textItem->setZValue(2);
-					// NO_HYPOTHESIS, 12
+
-					// MEMORY_IS_NULL, 13
+					_ui->imageView_loopClosure->setBackgroundBrush(QBrush(color));
-					// NOT_ENOUGH_MATCHING_PAIRS, 14
+					QImage img;
-					// EPIPOLAR_CONSTRAINT_FAILED, 15
+					QMap<int, QByteArray>::iterator iter = _imagesMap.find(highestHypothesisId);
-					// NULL_MATCHING_SURF_SIGNATURES 16
+					if(iter != _imagesMap.end())
-					// FUNDAMENTAL_MATRIX_NOT_FOUND 17
-					if(rejectLoopReason == 3 ||
-					   ((rejectLoopReason-10)>=0 && (rejectLoopReason-10) < 12))
 					{
-						QColor color;
+						if(!img.loadFromData(iter.value(), "JPEG"))
-						QGraphicsTextItem * textItem = 0;
-						switch(rejectLoopReason)
 						{
-						case 3:
+							ULOGGER_ERROR("conversion from QByteArray to QImage failed");
-							textItem = _ui->imageView_loopClosure->scene()->addText(tr("Rejected by hypothesis ratio"));
+							img = QImage();
-							color = Qt::yellow;
-							break;
-						case 13:
-							textItem = _ui->imageView_loopClosure->scene()->addText(tr("Memory is null (Intern error)"));
-							color = Qt::blue;
-							break;
-						case 14:
-							textItem = _ui->imageView_loopClosure->scene()->addText(tr("Not enough matching pairs"));
-							color = Qt::red;
-							break;
-						case 15:
-							textItem = _ui->imageView_loopClosure->scene()->addText(tr("Epipolar geometry verification failed"));
-							color = Qt::cyan;
-							break;
-						case 16:
-							textItem = _ui->imageView_loopClosure->scene()->addText(tr("Signatures are not the same type"));
-							color = Qt::magenta;
-							break;
-						case 17:
-							textItem = _ui->imageView_loopClosure->scene()->addText(tr("Fundamental matrix not found"));
-							color = Qt::yellow;
-							break;
-						default:
-							textItem = _ui->imageView_loopClosure->scene()->addText(tr("Undefined rejecting reason!?"));
-							color = Qt::blue;
-							break;
-						}
-						textItem->setDefaultTextColor(QColor(255-color.red(), 255-color.green(), 255-color.blue())); // color inverted
-						textItem->setZValue(2);
-
-						_ui->imageView_loopClosure->setBackgroundBrush(QBrush(color));
-						QImage img;
-						QMap<int, QByteArray>::iterator iter = _imagesMap.find(highestHypothesisId);
-						if(iter != _imagesMap.end())
-						{
-							if(!img.loadFromData(iter.value(), "JPEG"))
-							{
-								ULOGGER_ERROR("conversion from QByteArray to QImage failed");
-								img = QImage();
-							}
-						}
-						else if(stat.loopClosureImage())
-						{
-							img = Ipl2QImage(stat.loopClosureImage());
-						}
-
-						if(!img.isNull())
-						{
-							_ui->imageView_loopClosure->scene()->addPixmap(QPixmap::fromImage(img));
 						}
 					}
-					else
+					else if(stat.loopClosureImage())
 					{
-						ULOGGER_ERROR("rejectLoopReason=%d-10 is outside of the range of Qt::GlobalColor", rejectLoopReason);
+						img = Ipl2QImage(stat.loopClosureImage());
+					}
+
+					if(!img.isNull())
+					{
+						_ui->imageView_loopClosure->scene()->addPixmap(QPixmap::fromImage(img));
 					}
 				}
-			}
+				if(highestHypothesisIsSaved)
-			ULOGGER_DEBUG("");
+				{
-			if((stat.loopClosureId()>0 || rejectLoopReason>=12) && highestHypothesisIsSaved)
+					_ui->label_matchId->setText(QString("[Retrieved!] ").append(_ui->label_matchId->text()));
-			{
+				}
-				_ui->label_matchId->setText(QString("[Retrieved!] ").append(_ui->label_matchId->text()));
 			}
 
 			ULOGGER_DEBUG("");

guilib/src/PreferencesDialog.cpp

@@ -184,6 +184,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	// verifyHypotheses
 	_ui->comboBox_vh_strategy->setObjectName(Parameters::kRtabmapVhStrategy().c_str());
+	_ui->vh_doubleSpinBox_similarity->setObjectName(Parameters::kVhSimilarity().c_str());
 	_ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str());
 	_ui->surf_doubleSpinBox_ransacParam1->setObjectName(Parameters::kVhEpRansacParam1().c_str());
 	_ui->surf_doubleSpinBox_ransacParam2->setObjectName(Parameters::kVhEpRansacParam2().c_str());
@@ -1047,7 +1048,11 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 				{
 					// No panel related...
 				}
-				else if(value == 1) // 1 epipolar
+				else if(value == 1) // 1 similarity
+				{
+					this->addParameters(_ui->groupBox_vh_similarity);
+				}
+				else if(value == 2) // 2 epipolar
 				{
 					this->addParameters(_ui->groupBox_vh_epipolar);
 				}

guilib/src/ui/preferencesDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>892</width>
-    <height>792</height>
+    <height>796</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -46,9 +46,6 @@
        <attribute name="headerVisible">
         <bool>false</bool>
        </attribute>
-       <attribute name="headerVisible">
-        <bool>false</bool>
-       </attribute>
       </widget>
      </item>
      <item>
@@ -68,7 +65,7 @@
           <enum>QFrame::Raised</enum>
          </property>
          <property name="currentIndex">
-          <number>2</number>
+          <number>14</number>
          </property>
          <widget class="QWidget" name="page_22">
           <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1093,8 +1090,8 @@ We can edit how will like the prediction used in the Bayes filter when there is
                     <rect>
                      <x>0</x>
                      <y>0</y>
-                     <width>26</width>
+                     <width>98</width>
-                     <height>78</height>
+                     <height>71</height>
                     </rect>
                    </property>
                    <layout class="QHBoxLayout" name="horizontalLayout_2">
@@ -2710,6 +2707,11 @@ Lower the ratio -&gt; higher the precision</string>
                   <string>No verification</string>
                  </property>
                 </item>
+                <item>
+                 <property name="text">
+                  <string>Similarity</string>
+                 </property>
+                </item>
                 <item>
                  <property name="text">
                   <string>Epipolar constraints</string>
@@ -2742,6 +2744,58 @@ Lower the ratio -&gt; higher the precision</string>
            </item>
           </layout>
          </widget>
+         <widget class="QWidget" name="page_15">
+          <layout class="QVBoxLayout" name="verticalLayout_14">
+           <item>
+            <widget class="QGroupBox" name="groupBox_vh_similarity">
+             <property name="title">
+              <string>2Similarity verification</string>
+             </property>
+             <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,10">
+              <item row="0" column="0">
+               <widget class="QDoubleSpinBox" name="vh_doubleSpinBox_similarity">
+                <property name="decimals">
+                 <number>2</number>
+                </property>
+                <property name="maximum">
+                 <double>0.990000000000000</double>
+                </property>
+                <property name="singleStep">
+                 <double>0.010000000000000</double>
+                </property>
+                <property name="value">
+                 <double>0.500000000000000</double>
+                </property>
+               </widget>
+              </item>
+              <item row="0" column="1">
+               <widget class="QLabel" name="label_55">
+                <property name="text">
+                 <string>Minimum similarity to accept an hypothesis</string>
+                </property>
+                <property name="wordWrap">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </widget>
+           </item>
+           <item>
+            <spacer name="verticalSpacer_2">
+             <property name="orientation">
+              <enum>Qt::Vertical</enum>
+             </property>
+             <property name="sizeHint" stdset="0">
+              <size>
+               <width>20</width>
+               <height>619</height>
+              </size>
+             </property>
+            </spacer>
+           </item>
+          </layout>
+         </widget>
          <widget class="QWidget" name="page_21">
           <layout class="QVBoxLayout" name="verticalLayout_25">
            <item>