moved rtabmap-ros-pkg project in this project

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

corelib/src/BayesFilter.cpp

@@ -0,0 +1,462 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "BayesFilter.h"
+#include "Memory.h"
+#include "Signature.h"
+#include "rtabmap/core/Parameters.h"
+
+#include "utilite/UtiLite.h"
+
+namespace rtabmap {
+
+BayesFilter::BayesFilter(const ParametersMap & parameters) :
+	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr())
+{
+	this->setPredictionLC(Parameters::defaultBayesPredictionLC());
+	this->parseParameters(parameters);
+}
+
+BayesFilter::~BayesFilter() {
+}
+
+void BayesFilter::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kBayesVirtualPlacePriorThr())) != parameters.end())
+	{
+		this->setVirtualPlacePrior(std::atof((*iter).second.c_str()));
+	}
+	if((iter=parameters.find(Parameters::kBayesPredictionLC())) != parameters.end())
+	{
+		this->setPredictionLC((*iter).second);
+	}
+}
+
+void BayesFilter::setVirtualPlacePrior(float virtualPlacePrior)
+{
+	if(virtualPlacePrior < 0)
+	{
+		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
+		_virtualPlacePrior = 0;
+	}
+	else if(virtualPlacePrior > 1)
+	{
+		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
+		_virtualPlacePrior = 1;
+	}
+	else
+	{
+		_virtualPlacePrior = virtualPlacePrior;
+	}
+}
+
+// format = {Virtual place, Loop closure, level1, level2, l3, l4...}
+void BayesFilter::setPredictionLC(const std::string & prediction)
+{
+	std::list<std::string> strValues = uSplit(prediction, ' ');
+	if(strValues.size() < 2)
+	{
+		ULOGGER_ERROR("The number of values < 2 (prediction=\"%s\")", prediction.c_str());
+	}
+	else
+	{
+		std::vector<double> tmpValues(strValues.size());
+		int i=0;
+		bool valid = true;
+		float sum = 0;;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			tmpValues[i] = std::atof((*iter).c_str());
+			sum += tmpValues[i];
+			if(i>1)
+			{
+				sum += tmpValues[i]; // add a second time
+			}
+			if(tmpValues[i] < 0 || tmpValues[i]>1)
+			{
+				valid = false;
+				break;
+			}
+			++i;
+		}
+
+		if(!valid || sum <= 0 || sum > 1.001)
+		{
+			ULOGGER_ERROR("The prediction is not valid (the sum must be between >0 && <=1, sum=%f), negative values are not allowed (prediction=\"%s\")", sum, prediction.c_str());
+		}
+		else
+		{
+			_predictionLC = tmpValues;
+		}
+	}
+}
+
+const std::vector<double> & BayesFilter::getPredictionLC() const
+{
+	// {Vp, Lc, l1, l2, l3, l4...}
+	return _predictionLC;
+}
+
+std::string BayesFilter::getPredictionLCStr() const
+{
+	std::string values;
+	for(unsigned int i=0; i<_predictionLC.size(); ++i)
+	{
+		values.append(uNumber2str(_predictionLC[i]));
+		if(i+1 < _predictionLC.size())
+		{
+			values.append(" ");
+		}
+	}
+	return values;
+}
+
+void BayesFilter::reset()
+{
+	_posterior.clear();
+}
+
+const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
+{
+	ULOGGER_DEBUG("");
+
+	if(!memory)
+	{
+		ULOGGER_ERROR("Memory is Null!");
+		return _posterior;
+	}
+
+	if(!likelihood.size())
+	{
+		ULOGGER_ERROR("likelihood is empty!");
+		return _posterior;
+	}
+
+	if(_predictionLC.size() < 2)
+	{
+		ULOGGER_ERROR("Prediction is not valid!");
+		return _posterior;
+	}
+
+	UTimer timer;
+	timer.start();
+
+	CvMat * prediction = 0;
+	CvMat * prior = 0;
+	CvMat * posterior = 0;
+
+	float sum = 0;
+	int j=0;
+	// Recursive Bayes estimation...
+	// STEP 1 - Prediction : Prior*lastPosterior
+	prediction = cvCreateMat(likelihood.size(), likelihood.size(), CV_32FC1);
+	std::map<int, int> likelihoodKeys;
+	int index = 0;
+	for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
+	{
+		likelihoodKeys.insert(likelihoodKeys.end(), std::pair<int, int>(iter->first, index++));
+	}
+
+	if(this->generatePrediction(prediction, memory, likelihoodKeys))
+	{
+		ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), prediction->rows, prediction->cols);
+
+		// Adjust the last posterior if some images were
+		// reactivated or removed from the working memory
+		posterior = cvCreateMat(likelihood.size(), 1, CV_32FC1);
+		this->updatePosterior(memory, uKeys(likelihood));
+		j=0;
+		for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
+		{
+			posterior->data.fl[j++] = (*i).second;
+		}
+		ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior->rows, _posterior.size());
+
+		// Multiply prediction matrix with the last posterior
+		// (m,m) X (m,1) = (m,1)
+		prior = cvCreateMat(likelihood.size(), 1, CV_32FC1);
+		cvMatMul(prediction, posterior, prior);
+		ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
+
+		// STEP 2 - Update : Multiply with observations (likelihood)
+		j=0;
+		for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
+		{
+			std::map<int, float>::iterator p =_posterior.find((*i).first);
+			if(p!= _posterior.end())
+			{
+				(*p).second = (*i).second * prior->data.fl[j++];
+				sum+=(*p).second;
+			}
+			else
+			{
+				ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
+			}
+		}
+		ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
+
+		// Normalize
+		ULOGGER_DEBUG("sum=%f", sum);
+		if(sum != 0)
+		{
+			for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
+			{
+				(*i).second /= sum;
+			}
+		}
+		ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
+	}
+
+	cvReleaseMat(&prediction);
+	cvReleaseMat(&prior);
+	cvReleaseMat(&posterior);
+
+	return _posterior;
+}
+
+bool BayesFilter::generatePrediction(CvMat * prediction, const Memory * memory, const std::map<int, int> & likelihoodIds) const
+{
+	ULOGGER_DEBUG("");
+	UTimer timer;
+	timer.start();
+	UTimer timerGlobal;
+	timerGlobal.start();
+
+	if(!likelihoodIds.size() ||
+	   prediction == 0 ||
+	   prediction->rows != prediction->cols ||
+	   (unsigned int)prediction->rows != likelihoodIds.size()/*||
+	   prediction->type != CV_32FC1*/ ||
+	   _predictionLC.size() < 2 ||
+	   !memory)
+	{
+		ULOGGER_ERROR( "fail");
+		return false;
+	}
+
+	//int rows = prediction->rows;
+	cvSetZero(prediction);
+	int cols = prediction->cols;
+
+	// Each priors are column vectors
+	unsigned int i=0;
+	ULOGGER_DEBUG("_predictionLC.size()=%d",_predictionLC.size());
+	for(std::map<int, int>::const_iterator iter=likelihoodIds.begin(); iter!=likelihoodIds.end(); ++iter)
+	{
+		if(iter->first > 0)
+		{
+			// Create the sum of 2 gaussians around the loop closure
+
+			int loopClosureId = iter->first;
+
+			// Set high values (gaussians curves) to loop closure neighbors
+			const Signature * loopSign = memory->getSignature(loopClosureId);
+			if(!loopSign)
+			{
+				ULOGGER_ERROR("loopSign %d is not found?!?", loopClosureId);
+			}
+
+			// LoopID
+			prediction->data.fl[i + i*cols] += _predictionLC[1];
+
+			// look up for each neighbors (RECURSIVE)
+			this->addNeighborProb(prediction, i, memory, likelihoodIds, loopSign, 1);
+
+			//ULOGGER_DEBUG("neighbor prob for %d, neighbors=%d, time = %fs", loopSign->id(), loopSign->getNeighborIds().size(), timer.ticks());
+
+			float totalModelValues = _predictionLC[0] + _predictionLC[1];
+			for(unsigned int j=2; j<_predictionLC.size(); ++j)
+			{
+				totalModelValues += _predictionLC[j]*2;
+			}
+
+			//Add values of not found neighbors to the loop closure
+			float sum = 0;
+			for(int j=0; j<cols; ++j)
+			{
+				sum += prediction->data.fl[i + j*cols];
+			}
+			if(sum < (totalModelValues-_predictionLC[0]))
+			{
+				float gap = (totalModelValues-_predictionLC[0]) - sum;
+				prediction->data.fl[i + i*cols] += gap;
+				sum += gap;
+			}
+
+			// add virtual place prob
+			if(likelihoodIds.begin()->first < 0)
+			{
+				sum += prediction->data.fl[i] = _predictionLC[0];
+			}
+
+			// Set all loop events to small values according to the model
+			if(totalModelValues < 1.0f)
+			{
+				float value = (1.0f-totalModelValues) / float(cols);
+				for(int j=0; j<cols; ++j)
+				{
+					if(!prediction->data.fl[i + j*cols])
+					{
+						sum += prediction->data.fl[i + j*cols] = value;
+					}
+				}
+			}
+
+			//normalize this row,
+			for(int j=0; j<cols; ++j)
+			{
+				prediction->data.fl[i + j*cols] /= sum;
+			}
+
+			//debug
+			//for(int j=0; j<cols; ++j)
+			//{
+			//	ULOGGER_DEBUG("test = %f", prediction->data.fl[i + j*cols]);
+			//}
+		}
+		else
+		{
+			// Set the virtual place prior
+			if(_virtualPlacePrior > 0)
+			{
+				if(cols>1) // The first must be the virtual place
+				{
+					prediction->data.fl[i] = _virtualPlacePrior;
+					float val = (1.0-_virtualPlacePrior)/(cols-1);
+					for(int j=1; j<cols; j++)
+					{
+						prediction->data.fl[i + j*cols] = val;
+					}
+				}
+				else if(cols>0)
+				{
+					prediction->data.fl[i] = 1;
+				}
+			}
+			else
+			{
+				// Only for some tests...
+				// when _virtualPlacePrior=0, set all priors to the same value
+				if(cols>1)
+				{
+					float val = 1.0/cols;
+					for(int j=0; j<cols; j++)
+					{
+						prediction->data.fl[i + j*cols] = val;
+					}
+				}
+				else if(cols>0)
+				{
+					prediction->data.fl[i] = 1;
+				}
+			}
+		}
+		++i;
+	}
+
+	ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
+
+	return true;
+}
+
+void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
+{
+	ULOGGER_DEBUG("");
+	std::map<int, float> newPosterior;
+	for(std::vector<int>::const_iterator i=likelihoodIds.begin(); i != likelihoodIds.end(); ++i)
+	{
+		std::map<int, float>::iterator post = _posterior.find(*i);
+		if(post == _posterior.end())
+		{
+			if(_posterior.size() == 0)
+			{
+				newPosterior.insert(std::pair<int, float>(*i, 1));
+			}
+			else
+			{
+				newPosterior.insert(std::pair<int, float>(*i, 0));
+			}
+		}
+		else
+		{
+			newPosterior.insert(std::pair<int, float>((*post).first, (*post).second));
+		}
+	}
+	_posterior = newPosterior;
+}
+
+//recursive...
+float BayesFilter::addNeighborProb(CvMat * prediction, unsigned int row, const Memory * memory, const std::map<int, int> & likelihoodIds, const Signature * s, unsigned int level) const
+{
+	if(!likelihoodIds.size() ||
+	   prediction == 0 ||
+	   prediction->rows != prediction->cols ||
+	   (unsigned int)prediction->rows != likelihoodIds.size() ||
+	   _predictionLC.size() < 2 ||
+	   !memory ||
+	   !prediction ||
+	   level<1)
+	{
+		ULOGGER_ERROR( "fail");
+		return 0;
+	}
+
+	if(level+1 >= _predictionLC.size() || !s)
+	{
+		return 0;
+	}
+
+	double value = _predictionLC[level+1];
+	float sum=0;
+
+	const NeighborsMap & neighbors = s->getNeighbors();
+	for(NeighborsMap::const_iterator iter=neighbors.begin(); iter!= neighbors.end(); ++iter)
+	{
+		int index = uValue(likelihoodIds, iter->first, -1);
+		if(index >= 0)
+		{
+			bool alreadyAdded = false;
+			// the value can be already added in the recursion
+			if(value > prediction->data.fl[row + index*prediction->cols])
+			{
+				sum -= prediction->data.fl[row + index*prediction->cols];
+				prediction->data.fl[row + index*prediction->cols] = value;
+				sum += value;
+			}
+			else
+			{
+				alreadyAdded = true;
+			}
+
+			if(!alreadyAdded && level+1 < _predictionLC.size())
+			{
+				sum += addNeighborProb(prediction, row, memory, likelihoodIds, memory->getSignature(iter->first), level+1);
+			}
+		}
+		else
+		{
+			//ULOGGER_DEBUG("BayesFilter::generatePrediction(...) F (id %d) Not found for loop %d", loopSign->getNeighborForward(), loopClosureId);
+		}
+	}
+	return sum;
+}
+
+
+} // namespace rtabmap

corelib/src/BayesFilter.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef BAYESFILTER_H_
+#define BAYESFILTER_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <list>
+#include <set>
+#include "utilite/UEventsHandler.h"
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap {
+
+class Memory;
+class Signature;
+
+class RTABMAP_EXP BayesFilter
+{
+public:
+	BayesFilter(const ParametersMap & parameters = ParametersMap());
+	virtual ~BayesFilter();
+	virtual void parseParameters(const ParametersMap & parameters);
+	const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+	void reset();
+
+	//setters
+	void setVirtualPlacePrior(float virtualPlacePrior);
+	void setPredictionLC(const std::string & prediction);
+
+	//getters
+	const std::map<int, float> & getPosterior() const {return _posterior;}
+	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
+	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
+	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
+
+	bool generatePrediction(CvMat * prediction, const Memory * memory, const std::map<int, int> & likelihoodIds) const;
+	float addNeighborProb(CvMat * prediction, unsigned int row,  const Memory * memory, const std::map<int, int> & likelihoodIds, const Signature * s, unsigned int level) const;
+
+private:
+	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+
+private:
+	std::map<int, float> _posterior;
+	float _virtualPlacePrior;
+	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
+};
+
+} // namespace rtabmap
+
+#endif /* BAYESFILTER_H_ */

corelib/src/CMakeLists.txt

@@ -0,0 +1,77 @@
+
+SET(SRC_FILES
+	Rtabmap.cpp
+	RtabmapEvent.cpp
+	
+	Memory.cpp
+	KeypointMemory.cpp
+	
+	DBDriverFactory.cpp
+	DBDriver.cpp
+	DBDriverSqlite3.cpp
+	
+    Camera.cpp
+    EpipolarGeometry.cpp
+	VisualWord.cpp
+	VWDictionary.cpp
+	BayesFilter.cpp
+	Parameters.cpp
+    Signature.cpp
+	KeypointDetector.cpp
+	KeypointDescriptor.cpp
+	VerifyHypotheses.cpp
+	NearestNeighbor.cpp
+)
+
+SET(INCLUDE_DIRS
+	${CMAKE_CURRENT_SOURCE_DIR}/../include
+	${CMAKE_CURRENT_SOURCE_DIR}
+	${CMAKE_CURRENT_BINARY_DIR}
+	${UTILITE_INCLUDE_DIR}
+    ${OpenCV_INCLUDE_DIRS}
+	${SQLITE3_INCLUDE_DIR}
+)
+
+SET(LIBRARIES
+    ${UTILITE_LIBRARY}
+	${OpenCV_LIBS} 
+	${SQLITE3_LIBRARY}
+)
+
+# Generate resources files
+ADD_CUSTOM_COMMAND(
+   OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
+   COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+   COMMENT "[Creating resources]"
+)
+
+# Make sure the compiler can find include files from our library.
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+IF(WIN32)
+    IF(BUILD_SHARED_LIBS)   
+	   ADD_DEFINITIONS(-DRTABMAP_EXPORTS) 
+	ELSE()
+	   ADD_DEFINITIONS(-DRTABMAP_EXPORTS_STATIC)
+	ENDIF()
+ENDIF(WIN32)
+
+# Add binary that is built from the source file "main.cpp".
+# The extension is automatically found.
+ADD_LIBRARY(corelib ${SRC_FILES} ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h)
+TARGET_LINK_LIBRARIES(corelib ${LIBRARIES})
+
+SET_TARGET_PROPERTIES(
+corelib 
+PROPERTIES
+ OUTPUT_NAME ${PROJECT_PREFIX}_core
+ INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
+)
+
+INSTALL(TARGETS corelib
+        RUNTIME DESTINATION bin COMPONENT runtime
+        LIBRARY DESTINATION lib COMPONENT devel
+        ARCHIVE DESTINATION lib COMPONENT devel)
+		
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
+

corelib/src/Camera.cpp

@@ -0,0 +1,651 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/CameraEvent.h"
+#include "utilite/UEventsManager.h"
+#include "utilite/UConversion.h"
+#include "rtabmap/core/DBDriver.h"
+#include "rtabmap/core/DBDriverFactory.h"
+#include "rtabmap/core/KeypointDescriptor.h"
+#include "rtabmap/core/KeypointDetector.h"
+#include "rtabmap/core/SMState.h"
+#include "utilite/UStl.h"
+#include "utilite/UConversion.h"
+#include "utilite/UFile.h"
+#include "utilite/UDirectory.h"
+#include "utilite/UTimer.h"
+#include <opencv2/imgproc/imgproc_c.h>
+
+namespace rtabmap
+{
+
+SMState * CamPostTreatment::process(IplImage * image)
+{
+	if(image)
+	{
+		return new SMState(image);
+	}
+	return 0;
+}
+
+CamKeypointTreatment::~CamKeypointTreatment()
+{
+	if(_keypointDetector)
+	{
+		delete _keypointDetector;
+	}
+	if(_keypointDescriptor)
+	{
+		delete _keypointDescriptor;
+	}
+}
+SMState * CamKeypointTreatment::process(IplImage * image)
+{
+	if(image)
+	{
+		std::list<cv::KeyPoint> keypoints = _keypointDetector->generateKeypoints(image);
+		std::list<std::vector<float> > descriptors = _keypointDescriptor->generateDescriptors(image, keypoints);
+		SMState * smState = new SMState(descriptors, std::list<std::vector<float> >(), image, keypoints);
+		return smState;
+	}
+	return 0;
+}
+
+void CamKeypointTreatment::parseParameters(const ParametersMap & parameters)
+{
+	UDEBUG("");
+	ParametersMap::const_iterator iter;
+	//Keypoint detector
+	DetectorStrategy detectorStrategy = kDetectorUndef;
+	if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
+	{
+		detectorStrategy = (DetectorStrategy)std::atoi((*iter).second.c_str());
+	}
+	DetectorStrategy currentDetectorStrategy = this->detectorStrategy();
+	if(!_keypointDetector || ( detectorStrategy!=kDetectorUndef && (detectorStrategy != currentDetectorStrategy) ) )
+	{
+		ULOGGER_DEBUG("new detector strategy %d", int(detectorStrategy));
+		if(_keypointDetector)
+		{
+			delete _keypointDetector;
+			_keypointDetector = 0;
+		}
+		switch(detectorStrategy)
+		{
+		case kDetectorStar:
+			_keypointDetector = new StarDetector(parameters);
+			break;
+		case kDetectorSift:
+			_keypointDetector = new SIFTDetector(parameters);
+			break;
+		case kDetectorSurf:
+		default:
+			_keypointDetector = new SURFDetector(parameters);
+			break;
+		}
+	}
+	else if(_keypointDetector)
+	{
+		_keypointDetector->parseParameters(parameters);
+	}
+
+	//Keypoint descriptor
+	DescriptorStrategy descriptorStrategy = kDescriptorUndef;
+	if((iter=parameters.find(Parameters::kKpDescriptorStrategy())) != parameters.end())
+	{
+		descriptorStrategy = (DescriptorStrategy)std::atoi((*iter).second.c_str());
+	}
+	if(!_keypointDescriptor || descriptorStrategy!=kDescriptorUndef)
+	{
+		ULOGGER_DEBUG("new descriptor strategy %d", int(descriptorStrategy));
+		if(_keypointDescriptor)
+		{
+			delete _keypointDescriptor;
+			_keypointDescriptor = 0;
+		}
+		switch(descriptorStrategy)
+		{
+		case kDescriptorColorSurf:
+			// see decorator pattern...
+			_keypointDescriptor = new ColorDescriptor(parameters, new SURFDescriptor(parameters));
+			break;
+		case kDescriptorLaplacianSurf:
+			// see decorator pattern...
+			_keypointDescriptor = new LaplacianDescriptor(parameters, new SURFDescriptor(parameters));
+			break;
+		case kDescriptorSift:
+			_keypointDescriptor = new SIFTDescriptor(parameters);
+			break;
+		case kDescriptorHueSurf:
+			// see decorator pattern...
+			_keypointDescriptor = new HueDescriptor(parameters, new SURFDescriptor(parameters));
+			break;
+		case kDescriptorSurf:
+		default:
+			_keypointDescriptor = new SURFDescriptor(parameters);
+			break;
+		}
+	}
+	else if(_keypointDescriptor)
+	{
+		_keypointDescriptor->parseParameters(parameters);
+	}
+	CamPostTreatment::parseParameters(parameters);
+}
+
+CamKeypointTreatment::DetectorStrategy CamKeypointTreatment::detectorStrategy() const
+{
+	DetectorStrategy strategy = kDetectorUndef;
+	StarDetector * star = dynamic_cast<StarDetector*>(_keypointDetector);
+	SURFDetector * surf = dynamic_cast<SURFDetector*>(_keypointDetector);
+	if(star)
+	{
+		strategy = kDetectorStar;
+	}
+	else if(surf)
+	{
+		strategy = kDetectorSurf;
+	}
+	return strategy;
+}
+
+Camera::Camera(float imageRate,
+		bool autoRestart,
+		unsigned int imageWidth,
+		unsigned int imageHeight) :
+	_imageRate(imageRate),
+	_autoRestart(autoRestart),
+	_imageWidth(imageWidth),
+	_imageHeight(imageHeight)
+{
+	_postThreatement = new CamPostTreatment();
+	UEventsManager::addHandler(this);
+}
+
+Camera::~Camera(void)
+{
+	this->kill();
+	delete _postThreatement;
+}
+
+void Camera::mainLoop()
+{
+	State state = kStateCapturing;
+	ParametersMap parameters;
+
+	_stateMutex.lock();
+	{
+		if(!_state.empty() && !_stateParam.empty())
+		{
+			state = _state.top();
+			_state.pop();
+			parameters = _stateParam.top();
+			_stateParam.pop();
+		}
+	}
+	_stateMutex.unlock();
+
+	if(state == kStateCapturing)
+	{
+		process();
+	}
+	else if(state == kStateChangingParameters)
+	{
+		this->parseParameters(parameters);
+	}
+}
+
+// ownership is transferred
+void Camera::setPostThreatement(CamPostTreatment * strategy)
+{
+	if(strategy)
+	{
+		delete _postThreatement;
+		_postThreatement = strategy;
+	}
+}
+
+void Camera::pushNewState(State newState, const ParametersMap & parameters)
+{
+	ULOGGER_DEBUG("to %d", newState);
+
+	_stateMutex.lock();
+	{
+		_state.push(newState);
+		_stateParam.push(parameters);
+	}
+	_stateMutex.unlock();
+}
+
+void Camera::handleEvent(UEvent* anEvent)
+{
+	if(anEvent->getClassName().compare("CameraEvent") == 0)
+	{
+		CameraEvent * cameraEvent = (CameraEvent*)anEvent;
+		if(cameraEvent->getCode() == CameraEvent::kCodeCtrl)
+		{
+			CameraEvent::Cmd cmd = cameraEvent->getCommand();
+
+			if(cmd == CameraEvent::kCmdPause)
+			{
+				if(this->isRunning())
+				{
+					this->kill();
+				}
+				else
+				{
+					this->start();
+				}
+			}
+			else if(cmd == CameraEvent::kCmdChangeParam)
+			{
+				// TODO : Put in global Parameters ?
+				_imageRate = cameraEvent->getImageRate();
+				_autoRestart = cameraEvent->getAutoRestart();
+			}
+			else
+			{
+				ULOGGER_DEBUG("Camera::handleEvent(Util::Event* anEvent) : command undefined...");
+			}
+		}
+	}
+	if(anEvent->getClassName().compare("ParamEvent") == 0)
+	{
+		if(this->isIdle())
+		{
+			_stateMutex.lock();
+			this->parseParameters(((ParamEvent*)anEvent)->getParameters());
+			_stateMutex.unlock();
+		}
+		else
+		{
+			ULOGGER_DEBUG("changing parameters");
+			pushNewState(kStateChangingParameters, ((ParamEvent*)anEvent)->getParameters());
+		}
+	}
+}
+
+void Camera::process()
+{
+	UTimer timer;
+	ULOGGER_DEBUG("Camera::process()");
+	IplImage * image = this->takeImage();
+	if(image)
+	{
+		SMState * smState = _postThreatement->process(image);
+		this->post(new SMStateEvent(smState));
+		double elapsed = timer.ticks();
+		UDEBUG("Post treatment time = %fs", elapsed);
+		if(_imageRate>0)
+		{
+			float sleepTime = 1000.0f/_imageRate - 1000.0f*elapsed;
+			if(sleepTime > 0)
+			{
+				UDEBUG("Now sleeping for = %fms", sleepTime);
+				uSleep(sleepTime);
+			}
+		}
+	}
+	else
+	{
+		if(_autoRestart)
+		{
+			this->init();
+		}
+		else
+		{
+			ULOGGER_DEBUG("Camera::process() : no more images...");
+			this->kill();
+			this->post(new CameraEvent());
+		}
+	}
+}
+
+
+/////////////////////////
+// CameraImages
+/////////////////////////
+CameraImages::CameraImages(const std::string & path,
+					 int startAt,
+					 bool refreshDir,
+					 float imageRate,
+					 bool autoRestart,
+					 unsigned int imageWidth,
+					 unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_path(path),
+	_startAt(startAt),
+	_refreshDir(refreshDir),
+	_dir(0),
+	_count(0)
+{
+}
+
+CameraImages::~CameraImages(void)
+{
+	this->kill();
+	if(_dir)
+	{
+		delete _dir;
+		_dir = 0;
+	}
+}
+
+bool CameraImages::init()
+{
+	if(_dir)
+	{
+		delete _dir;
+		_dir = 0;
+	}
+	_dir = new UDirectory(_path, "jpg ppm png bmp");
+	_count = 0;
+	if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
+	{
+		_path.append("/");
+	}
+	if(!_dir)
+	{
+		ULOGGER_ERROR("Directory path not valid \"%s\"", _path.c_str());
+	}
+	return _dir != 0;
+}
+
+IplImage * CameraImages::takeImage()
+{
+	IplImage * img = 0;
+	if(_dir)
+	{
+		if(_refreshDir)
+		{
+			_dir->update();
+		}
+		if(_startAt == 0)
+		{
+			const std::list<std::string> & fileNames = _dir->getFileNames();
+			if(fileNames.size())
+			{
+				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
+				{
+					_lastFileName = *fileNames.rbegin();
+					std::string fullPath = _path + _lastFileName;
+					img = cvLoadImage(fullPath.c_str(), CV_LOAD_IMAGE_UNCHANGED);
+				}
+			}
+		}
+		else
+		{
+			std::string fileName;
+			std::string fullPath;
+			fileName = _dir->getNextFileName();
+			if(fileName.size())
+			{
+				fullPath = _path + fileName;
+				while(++_count < _startAt && (fileName = _dir->getNextFileName()).size())
+				{
+					fullPath = _path + fileName;
+				}
+				if(fileName.size())
+				{
+					ULOGGER_DEBUG("Loading image : %s\n", fullPath.c_str());
+					img = cvLoadImage(fullPath.c_str(), CV_LOAD_IMAGE_UNCHANGED);
+				}
+			}
+		}
+	}
+	if(img &&
+	   getImageWidth() &&
+	   getImageHeight() &&
+	   getImageWidth() != (unsigned int)img->width &&
+	   getImageHeight() != (unsigned int)img->height)
+	{
+		// declare a destination IplImage object with correct size, depth and channels
+		IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()) ,
+											   (int)(getImageHeight()) ),
+											   img->depth, img->nChannels );
+
+		//use cvResize to resize source to a destination image (linear interpolation)
+		cvResize(img, resampledImg);
+		cvReleaseImage(&img);
+		img = resampledImg;
+	}
+	return img;
+}
+
+
+
+/////////////////////////
+// CameraVideo
+/////////////////////////
+CameraVideo::CameraVideo(int usbDevice,
+						 float imageRate,
+						 bool autoRestart,
+						 unsigned int imageWidth,
+						  unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_capture(0),
+	_src(kUsbDevice),
+	_usbDevice(usbDevice)
+{
+
+}
+
+CameraVideo::CameraVideo(const std::string & fileName,
+						   float imageRate,
+						   bool autoRestart,
+						   unsigned int imageWidth,
+						   unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_fileName(fileName),
+	_capture(0),
+	_src(kVideoFile)
+{
+}
+
+CameraVideo::~CameraVideo()
+{
+	this->kill();
+	if(_capture)
+	{
+		cvReleaseCapture(&_capture);
+	}
+}
+
+bool CameraVideo::init()
+{
+	if(_capture)
+	{
+		cvReleaseCapture(&_capture);
+		_capture = 0;
+	}
+
+	if(_src == kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d with imgSize=[%d,%d]", _usbDevice, getImageWidth(), getImageHeight());
+		_capture = cvCaptureFromCAM(_usbDevice);
+		if(_capture && getImageWidth() && getImageHeight())
+		{
+			cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_WIDTH, double(getImageWidth()));
+			cvSetCaptureProperty(_capture, CV_CAP_PROP_FRAME_HEIGHT, double(getImageHeight()));
+		}
+	}
+	else if(_src == kVideoFile)
+	{
+		ULOGGER_DEBUG("CameraVideo::init() filename=\"%s\"", _fileName.c_str());
+		_capture = cvCaptureFromAVI(_fileName.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraVideo::init() Unknown source...");
+	}
+	if(!_capture)
+	{
+		ULOGGER_ERROR("CameraVideo::init() Failed to create a capture object!");
+		return false;
+	}
+	return true;
+}
+
+IplImage * CameraVideo::takeImage()
+{
+	IplImage * img = 0;  // Null image
+	if(_capture)
+	{
+		if(!cvGrabFrame(_capture)){              // capture a frame
+			ULOGGER_WARN("CameraVideo: Could not grab a frame, the end of the feed may be reached...");
+		}
+		else
+		{
+			img=cvRetrieveFrame(_capture);           // retrieve the captured frame
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("CameraVideo::takeImage() The camera must be initialized before requesting an image.");
+	}
+
+	if(img &&
+	   getImageWidth() &&
+	   getImageHeight() &&
+	   getImageWidth() != (unsigned int)img->width &&
+	   getImageHeight() != (unsigned int)img->height)
+	{
+		// declare a destination IplImage object with correct size, depth and channels
+		IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()) ,
+											   (int)(getImageHeight()) ),
+											   img->depth, img->nChannels );
+
+		//use cvResize to resize source to a destination image (linear interpolation)
+		cvResize(img, resampledImg);
+		img = resampledImg;
+	}
+	else if(img)
+	{
+		img = cvCloneImage(img);
+	}
+
+	return img;
+}
+
+
+
+
+
+
+
+/////////////////////////
+// CameraDatabase
+/////////////////////////
+CameraDatabase::CameraDatabase(const std::string & path,
+								bool ignoreChildren,
+								float imageRate,
+								bool autoRestart,
+								unsigned int imageWidth,
+								unsigned int imageHeight) :
+	Camera(imageRate, autoRestart, imageWidth, imageHeight),
+	_path(path),
+	_ignoreChildren(ignoreChildren),
+	_indexIter(_ids.begin()),
+	_dbDriver(0)
+{
+}
+
+CameraDatabase::~CameraDatabase(void)
+{
+	this->kill();
+	if(_dbDriver)
+	{
+		_dbDriver->closeConnection();
+		delete _dbDriver;
+	}
+}
+
+bool CameraDatabase::init()
+{
+	if(_dbDriver)
+	{
+		_dbDriver->closeConnection();
+		delete _dbDriver;
+		_dbDriver = 0;
+	}
+	_ids.clear();
+	_indexIter = _ids.begin();
+
+	std::string driverType = "sqlite3";
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+	_dbDriver = rtabmap::DBDriverFactory::createDBDriver(driverType, parameters);
+	if(!_dbDriver)
+	{
+		ULOGGER_ERROR("CameraDatabase::init() can't create \"%s\" driver",driverType.c_str());
+		return false;
+	}
+	else if(!_dbDriver->openConnection(_path.c_str()))
+	{
+		ULOGGER_ERROR("CameraDatabase::init() Can't read database \"%s\"",_path.c_str());
+		return false;
+	}
+	else
+	{
+		// TODO load all signatures only if ignoreChildren is false
+		_dbDriver->getAllSignatureIds(_ids);
+		_indexIter = _ids.begin();
+	}
+	return true;
+}
+
+IplImage * CameraDatabase::takeImage()
+{
+	IplImage * img = 0;
+	if(_dbDriver && _indexIter != _ids.end())
+	{
+		_dbDriver->getImage(*_indexIter, &img);
+		++_indexIter;
+	}
+	else if(!_dbDriver)
+	{
+		ULOGGER_WARN("The camera must be initialized first...");
+	}
+	else if(_ids.size() == 0)
+	{
+		ULOGGER_WARN("The database \"%s\" is empty...", _path.c_str());
+	}
+
+	if(img &&
+	   getImageWidth() &&
+	   getImageHeight() &&
+	   getImageWidth() != (unsigned int)img->width &&
+	   getImageHeight() != (unsigned int)img->height)
+	{
+		// declare a destination IplImage object with correct size, depth and channels
+		IplImage * resampledImg = cvCreateImage( cvSize((int)(getImageWidth()) ,
+											   (int)(getImageHeight()) ),
+											   img->depth, img->nChannels );
+
+		//use cvResize to resize source to a destination image (linear interpolation)
+		cvResize(img, resampledImg);
+		cvReleaseImage(&img);
+		img = resampledImg;
+	}
+
+	return img;
+}
+
+} // namespace rtabmap

corelib/src/ConvertUTF.c

@@ -0,0 +1,539 @@
+/*
+ * Copyright 2001-2004 Unicode, Inc.
+ * 
+ * Disclaimer
+ * 
+ * This source code is provided as is by Unicode, Inc. No claims are
+ * made as to fitness for any particular purpose. No warranties of any
+ * kind are expressed or implied. The recipient agrees to determine
+ * applicability of information provided. If this file has been
+ * purchased on magnetic or optical media from Unicode, Inc., the
+ * sole remedy for any claim will be exchange of defective media
+ * within 90 days of receipt.
+ * 
+ * Limitations on Rights to Redistribute This Code
+ * 
+ * Unicode, Inc. hereby grants the right to freely use the information
+ * supplied in this file in the creation of products supporting the
+ * Unicode Standard, and to make copies of this file in any form
+ * for internal or external distribution as long as this notice
+ * remains attached.
+ */
+
+/* ---------------------------------------------------------------------
+
+    Conversions between UTF32, UTF-16, and UTF-8. Source code file.
+    Author: Mark E. Davis, 1994.
+    Rev History: Rick McGowan, fixes & updates May 2001.
+    Sept 2001: fixed const & error conditions per
+	mods suggested by S. Parent & A. Lillich.
+    June 2002: Tim Dodd added detection and handling of incomplete
+	source sequences, enhanced error detection, added casts
+	to eliminate compiler warnings.
+    July 2003: slight mods to back out aggressive FFFE detection.
+    Jan 2004: updated switches in from-UTF8 conversions.
+    Oct 2004: updated to use UNI_MAX_LEGAL_UTF32 in UTF-32 conversions.
+
+    See the header file "ConvertUTF.h" for complete documentation.
+
+------------------------------------------------------------------------ */
+
+
+#include "ConvertUTF.h"
+#ifdef CVTUTF_DEBUG
+#include <stdio.h>
+#endif
+
+static const int halfShift  = 10; /* used for shifting by 10 bits */
+
+static const UTF32 halfBase = 0x0010000UL;
+static const UTF32 halfMask = 0x3FFUL;
+
+#define UNI_SUR_HIGH_START  (UTF32)0xD800
+#define UNI_SUR_HIGH_END    (UTF32)0xDBFF
+#define UNI_SUR_LOW_START   (UTF32)0xDC00
+#define UNI_SUR_LOW_END     (UTF32)0xDFFF
+#define false	   0
+#define true	    1
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF32toUTF16 (
+	const UTF32** sourceStart, const UTF32* sourceEnd, 
+	UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF32* source = *sourceStart;
+    UTF16* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch;
+	if (target >= targetEnd) {
+	    result = targetExhausted; break;
+	}
+	ch = *source++;
+	if (ch <= UNI_MAX_BMP) { /* Target is a character <= 0xFFFF */
+	    /* UTF-16 surrogate values are illegal in UTF-32; 0xffff or 0xfffe are both reserved values */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		if (flags == strictConversion) {
+		    --source; /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		} else {
+		    *target++ = UNI_REPLACEMENT_CHAR;
+		}
+	    } else {
+		*target++ = (UTF16)ch; /* normal case */
+	    }
+	} else if (ch > UNI_MAX_LEGAL_UTF32) {
+	    if (flags == strictConversion) {
+		result = sourceIllegal;
+	    } else {
+		*target++ = UNI_REPLACEMENT_CHAR;
+	    }
+	} else {
+	    /* target is a character in range 0xFFFF - 0x10FFFF. */
+	    if (target + 1 >= targetEnd) {
+		--source; /* Back up source pointer! */
+		result = targetExhausted; break;
+	    }
+	    ch -= halfBase;
+	    *target++ = (UTF16)((ch >> halfShift) + UNI_SUR_HIGH_START);
+	    *target++ = (UTF16)((ch & halfMask) + UNI_SUR_LOW_START);
+	}
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF16toUTF32 (
+	const UTF16** sourceStart, const UTF16* sourceEnd, 
+	UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF16* source = *sourceStart;
+    UTF32* target = *targetStart;
+    UTF32 ch, ch2;
+    while (source < sourceEnd) {
+	const UTF16* oldSource = source; /*  In case we have to back up because of target overflow. */
+	ch = *source++;
+	/* If we have a surrogate pair, convert to UTF32 first. */
+	if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_HIGH_END) {
+	    /* If the 16 bits following the high surrogate are in the source buffer... */
+	    if (source < sourceEnd) {
+		ch2 = *source;
+		/* If it's a low surrogate, convert to UTF32. */
+		if (ch2 >= UNI_SUR_LOW_START && ch2 <= UNI_SUR_LOW_END) {
+		    ch = ((ch - UNI_SUR_HIGH_START) << halfShift)
+			+ (ch2 - UNI_SUR_LOW_START) + halfBase;
+		    ++source;
+		} else if (flags == strictConversion) { /* it's an unpaired high surrogate */
+		    --source; /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		}
+	    } else { /* We don't have the 16 bits following the high surrogate. */
+		--source; /* return to the high surrogate */
+		result = sourceExhausted;
+		break;
+	    }
+	} else if (flags == strictConversion) {
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_LOW_START && ch <= UNI_SUR_LOW_END) {
+		--source; /* return to the illegal value itself */
+		result = sourceIllegal;
+		break;
+	    }
+	}
+	if (target >= targetEnd) {
+	    source = oldSource; /* Back up source pointer! */
+	    result = targetExhausted; break;
+	}
+	*target++ = ch;
+    }
+    *sourceStart = source;
+    *targetStart = target;
+#ifdef CVTUTF_DEBUG
+if (result == sourceIllegal) {
+    fprintf(stderr, "ConvertUTF16toUTF32 illegal seq 0x%04x,%04x\n", ch, ch2);
+    fflush(stderr);
+}
+#endif
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Index into the table below with the first byte of a UTF-8 sequence to
+ * get the number of trailing bytes that are supposed to follow it.
+ * Note that *legal* UTF-8 values can't have 4 or 5-bytes. The table is
+ * left as-is for anyone who may want to do such conversion, which was
+ * allowed in earlier algorithms.
+ */
+static const char trailingBytesForUTF8[256] = {
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
+    1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
+    2,2,2,2,2,2,2,2,2,2,2,2,2,2,2,2, 3,3,3,3,3,3,3,3,4,4,4,4,5,5,5,5
+};
+
+/*
+ * Magic values subtracted from a buffer value during UTF8 conversion.
+ * This table contains as many values as there might be trailing bytes
+ * in a UTF-8 sequence.
+ */
+static const UTF32 offsetsFromUTF8[6] = { 0x00000000UL, 0x00003080UL, 0x000E2080UL, 
+		     0x03C82080UL, 0xFA082080UL, 0x82082080UL };
+
+/*
+ * Once the bits are split out into bytes of UTF-8, this is a mask OR-ed
+ * into the first byte, depending on how many bytes follow.  There are
+ * as many entries in this table as there are UTF-8 sequence types.
+ * (I.e., one byte sequence, two byte... etc.). Remember that sequencs
+ * for *legal* UTF-8 will be 4 or fewer bytes total.
+ */
+static const UTF8 firstByteMark[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC };
+
+/* --------------------------------------------------------------------- */
+
+/* The interface converts a whole buffer to avoid function-call overhead.
+ * Constants have been gathered. Loops & conditionals have been removed as
+ * much as possible for efficiency, in favor of drop-through switches.
+ * (See "Note A" at the bottom of the file for equivalent code.)
+ * If your compiler supports it, the "isLegalUTF8" call can be turned
+ * into an inline function.
+ */
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF16toUTF8 (
+	const UTF16** sourceStart, const UTF16* sourceEnd, 
+	UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF16* source = *sourceStart;
+    UTF8* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch;
+	unsigned short bytesToWrite = 0;
+	const UTF32 byteMask = 0xBF;
+	const UTF32 byteMark = 0x80; 
+	const UTF16* oldSource = source; /* In case we have to back up because of target overflow. */
+	ch = *source++;
+	/* If we have a surrogate pair, convert to UTF32 first. */
+	if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_HIGH_END) {
+	    /* If the 16 bits following the high surrogate are in the source buffer... */
+	    if (source < sourceEnd) {
+		UTF32 ch2 = *source;
+		/* If it's a low surrogate, convert to UTF32. */
+		if (ch2 >= UNI_SUR_LOW_START && ch2 <= UNI_SUR_LOW_END) {
+		    ch = ((ch - UNI_SUR_HIGH_START) << halfShift)
+			+ (ch2 - UNI_SUR_LOW_START) + halfBase;
+		    ++source;
+		} else if (flags == strictConversion) { /* it's an unpaired high surrogate */
+		    --source; /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		}
+	    } else { /* We don't have the 16 bits following the high surrogate. */
+		--source; /* return to the high surrogate */
+		result = sourceExhausted;
+		break;
+	    }
+	} else if (flags == strictConversion) {
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_LOW_START && ch <= UNI_SUR_LOW_END) {
+		--source; /* return to the illegal value itself */
+		result = sourceIllegal;
+		break;
+	    }
+	}
+	/* Figure out how many bytes the result will require */
+	if (ch < (UTF32)0x80) {	     bytesToWrite = 1;
+	} else if (ch < (UTF32)0x800) {     bytesToWrite = 2;
+	} else if (ch < (UTF32)0x10000) {   bytesToWrite = 3;
+	} else if (ch < (UTF32)0x110000) {  bytesToWrite = 4;
+	} else {			    bytesToWrite = 3;
+					    ch = UNI_REPLACEMENT_CHAR;
+	}
+
+	target += bytesToWrite;
+	if (target > targetEnd) {
+	    source = oldSource; /* Back up source pointer! */
+	    target -= bytesToWrite; result = targetExhausted; break;
+	}
+	switch (bytesToWrite) { /* note: everything falls through. */
+	    case 4: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 3: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 2: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 1: *--target =  (UTF8)(ch | firstByteMark[bytesToWrite]);
+	}
+	target += bytesToWrite;
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Utility routine to tell whether a sequence of bytes is legal UTF-8.
+ * This must be called with the length pre-determined by the first byte.
+ * If not calling this from ConvertUTF8to*, then the length can be set by:
+ *  length = trailingBytesForUTF8[*source]+1;
+ * and the sequence is illegal right away if there aren't that many bytes
+ * available.
+ * If presented with a length > 4, this returns false.  The Unicode
+ * definition of UTF-8 goes up to 4-byte sequences.
+ */
+
+static Boolean isLegalUTF8(const UTF8 *source, int length) {
+    UTF8 a;
+    const UTF8 *srcptr = source+length;
+    switch (length) {
+    default: return false;
+	/* Everything else falls through when "true"... */
+    case 4: if ((a = (*--srcptr)) < 0x80 || a > 0xBF) return false;
+    case 3: if ((a = (*--srcptr)) < 0x80 || a > 0xBF) return false;
+    case 2: if ((a = (*--srcptr)) > 0xBF) return false;
+
+	switch (*source) {
+	    /* no fall-through in this inner switch */
+	    case 0xE0: if (a < 0xA0) return false; break;
+	    case 0xED: if (a > 0x9F) return false; break;
+	    case 0xF0: if (a < 0x90) return false; break;
+	    case 0xF4: if (a > 0x8F) return false; break;
+	    default:   if (a < 0x80) return false;
+	}
+
+    case 1: if (*source >= 0x80 && *source < 0xC2) return false;
+    }
+    if (*source > 0xF4) return false;
+    return true;
+}
+
+/* --------------------------------------------------------------------- */
+
+/*
+ * Exported function to return whether a UTF-8 sequence is legal or not.
+ * This is not used here; it's just exported.
+ */
+Boolean isLegalUTF8Sequence(const UTF8 *source, const UTF8 *sourceEnd) {
+    int length = trailingBytesForUTF8[*source]+1;
+    if (source+length > sourceEnd) {
+	return false;
+    }
+    return isLegalUTF8(source, length);
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF8toUTF16 (
+	const UTF8** sourceStart, const UTF8* sourceEnd, 
+	UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF8* source = *sourceStart;
+    UTF16* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch = 0;
+	unsigned short extraBytesToRead = trailingBytesForUTF8[*source];
+	if (source + extraBytesToRead >= sourceEnd) {
+	    result = sourceExhausted; break;
+	}
+	/* Do this check whether lenient or strict */
+	if (! isLegalUTF8(source, extraBytesToRead+1)) {
+	    result = sourceIllegal;
+	    break;
+	}
+	/*
+	 * The cases all fall through. See "Note A" below.
+	 */
+	switch (extraBytesToRead) {
+	    case 5: ch += *source++; ch <<= 6; /* remember, illegal UTF-8 */
+	    case 4: ch += *source++; ch <<= 6; /* remember, illegal UTF-8 */
+	    case 3: ch += *source++; ch <<= 6;
+	    case 2: ch += *source++; ch <<= 6;
+	    case 1: ch += *source++; ch <<= 6;
+	    case 0: ch += *source++;
+	}
+	ch -= offsetsFromUTF8[extraBytesToRead];
+
+	if (target >= targetEnd) {
+	    source -= (extraBytesToRead+1); /* Back up source pointer! */
+	    result = targetExhausted; break;
+	}
+	if (ch <= UNI_MAX_BMP) { /* Target is a character <= 0xFFFF */
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		if (flags == strictConversion) {
+		    source -= (extraBytesToRead+1); /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		} else {
+		    *target++ = UNI_REPLACEMENT_CHAR;
+		}
+	    } else {
+		*target++ = (UTF16)ch; /* normal case */
+	    }
+	} else if (ch > UNI_MAX_UTF16) {
+	    if (flags == strictConversion) {
+		result = sourceIllegal;
+		source -= (extraBytesToRead+1); /* return to the start */
+		break; /* Bail out; shouldn't continue */
+	    } else {
+		*target++ = UNI_REPLACEMENT_CHAR;
+	    }
+	} else {
+	    /* target is a character in range 0xFFFF - 0x10FFFF. */
+	    if (target + 1 >= targetEnd) {
+		source -= (extraBytesToRead+1); /* Back up source pointer! */
+		result = targetExhausted; break;
+	    }
+	    ch -= halfBase;
+	    *target++ = (UTF16)((ch >> halfShift) + UNI_SUR_HIGH_START);
+	    *target++ = (UTF16)((ch & halfMask) + UNI_SUR_LOW_START);
+	}
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF32toUTF8 (
+	const UTF32** sourceStart, const UTF32* sourceEnd, 
+	UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF32* source = *sourceStart;
+    UTF8* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch;
+	unsigned short bytesToWrite = 0;
+	const UTF32 byteMask = 0xBF;
+	const UTF32 byteMark = 0x80; 
+	ch = *source++;
+	if (flags == strictConversion ) {
+	    /* UTF-16 surrogate values are illegal in UTF-32 */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		--source; /* return to the illegal value itself */
+		result = sourceIllegal;
+		break;
+	    }
+	}
+	/*
+	 * Figure out how many bytes the result will require. Turn any
+	 * illegally large UTF32 things (> Plane 17) into replacement chars.
+	 */
+	if (ch < (UTF32)0x80) {	     bytesToWrite = 1;
+	} else if (ch < (UTF32)0x800) {     bytesToWrite = 2;
+	} else if (ch < (UTF32)0x10000) {   bytesToWrite = 3;
+	} else if (ch <= UNI_MAX_LEGAL_UTF32) {  bytesToWrite = 4;
+	} else {			    bytesToWrite = 3;
+					    ch = UNI_REPLACEMENT_CHAR;
+					    result = sourceIllegal;
+	}
+	
+	target += bytesToWrite;
+	if (target > targetEnd) {
+	    --source; /* Back up source pointer! */
+	    target -= bytesToWrite; result = targetExhausted; break;
+	}
+	switch (bytesToWrite) { /* note: everything falls through. */
+	    case 4: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 3: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 2: *--target = (UTF8)((ch | byteMark) & byteMask); ch >>= 6;
+	    case 1: *--target = (UTF8) (ch | firstByteMark[bytesToWrite]);
+	}
+	target += bytesToWrite;
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* --------------------------------------------------------------------- */
+
+ConversionResult ConvertUTF8toUTF32 (
+	const UTF8** sourceStart, const UTF8* sourceEnd, 
+	UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags) {
+    ConversionResult result = conversionOK;
+    const UTF8* source = *sourceStart;
+    UTF32* target = *targetStart;
+    while (source < sourceEnd) {
+	UTF32 ch = 0;
+	unsigned short extraBytesToRead = trailingBytesForUTF8[*source];
+	if (source + extraBytesToRead >= sourceEnd) {
+	    result = sourceExhausted; break;
+	}
+	/* Do this check whether lenient or strict */
+	if (! isLegalUTF8(source, extraBytesToRead+1)) {
+	    result = sourceIllegal;
+	    break;
+	}
+	/*
+	 * The cases all fall through. See "Note A" below.
+	 */
+	switch (extraBytesToRead) {
+	    case 5: ch += *source++; ch <<= 6;
+	    case 4: ch += *source++; ch <<= 6;
+	    case 3: ch += *source++; ch <<= 6;
+	    case 2: ch += *source++; ch <<= 6;
+	    case 1: ch += *source++; ch <<= 6;
+	    case 0: ch += *source++;
+	}
+	ch -= offsetsFromUTF8[extraBytesToRead];
+
+	if (target >= targetEnd) {
+	    source -= (extraBytesToRead+1); /* Back up the source pointer! */
+	    result = targetExhausted; break;
+	}
+	if (ch <= UNI_MAX_LEGAL_UTF32) {
+	    /*
+	     * UTF-16 surrogate values are illegal in UTF-32, and anything
+	     * over Plane 17 (> 0x10FFFF) is illegal.
+	     */
+	    if (ch >= UNI_SUR_HIGH_START && ch <= UNI_SUR_LOW_END) {
+		if (flags == strictConversion) {
+		    source -= (extraBytesToRead+1); /* return to the illegal value itself */
+		    result = sourceIllegal;
+		    break;
+		} else {
+		    *target++ = UNI_REPLACEMENT_CHAR;
+		}
+	    } else {
+		*target++ = ch;
+	    }
+	} else { /* i.e., ch > UNI_MAX_LEGAL_UTF32 */
+	    result = sourceIllegal;
+	    *target++ = UNI_REPLACEMENT_CHAR;
+	}
+    }
+    *sourceStart = source;
+    *targetStart = target;
+    return result;
+}
+
+/* ---------------------------------------------------------------------
+
+    Note A.
+    The fall-through switches in UTF-8 reading code save a
+    temp variable, some decrements & conditionals.  The switches
+    are equivalent to the following loop:
+	{
+	    int tmpBytesToRead = extraBytesToRead+1;
+	    do {
+		ch += *source++;
+		--tmpBytesToRead;
+		if (tmpBytesToRead) ch <<= 6;
+	    } while (tmpBytesToRead > 0);
+	}
+    In UTF-8 writing code, the switches on "bytesToWrite" are
+    similarly unrolled loops.
+
+   --------------------------------------------------------------------- */

corelib/src/ConvertUTF.h

@@ -0,0 +1,149 @@
+/*
+ * Copyright 2001-2004 Unicode, Inc.
+ * 
+ * Disclaimer
+ * 
+ * This source code is provided as is by Unicode, Inc. No claims are
+ * made as to fitness for any particular purpose. No warranties of any
+ * kind are expressed or implied. The recipient agrees to determine
+ * applicability of information provided. If this file has been
+ * purchased on magnetic or optical media from Unicode, Inc., the
+ * sole remedy for any claim will be exchange of defective media
+ * within 90 days of receipt.
+ * 
+ * Limitations on Rights to Redistribute This Code
+ * 
+ * Unicode, Inc. hereby grants the right to freely use the information
+ * supplied in this file in the creation of products supporting the
+ * Unicode Standard, and to make copies of this file in any form
+ * for internal or external distribution as long as this notice
+ * remains attached.
+ */
+
+/* ---------------------------------------------------------------------
+
+    Conversions between UTF32, UTF-16, and UTF-8.  Header file.
+
+    Several funtions are included here, forming a complete set of
+    conversions between the three formats.  UTF-7 is not included
+    here, but is handled in a separate source file.
+
+    Each of these routines takes pointers to input buffers and output
+    buffers.  The input buffers are const.
+
+    Each routine converts the text between *sourceStart and sourceEnd,
+    putting the result into the buffer between *targetStart and
+    targetEnd. Note: the end pointers are *after* the last item: e.g. 
+    *(sourceEnd - 1) is the last item.
+
+    The return result indicates whether the conversion was successful,
+    and if not, whether the problem was in the source or target buffers.
+    (Only the first encountered problem is indicated.)
+
+    After the conversion, *sourceStart and *targetStart are both
+    updated to point to the end of last text successfully converted in
+    the respective buffers.
+
+    Input parameters:
+	sourceStart - pointer to a pointer to the source buffer.
+		The contents of this are modified on return so that
+		it points at the next thing to be converted.
+	targetStart - similarly, pointer to pointer to the target buffer.
+	sourceEnd, targetEnd - respectively pointers to the ends of the
+		two buffers, for overflow checking only.
+
+    These conversion functions take a ConversionFlags argument. When this
+    flag is set to strict, both irregular sequences and isolated surrogates
+    will cause an error.  When the flag is set to lenient, both irregular
+    sequences and isolated surrogates are converted.
+
+    Whether the flag is strict or lenient, all illegal sequences will cause
+    an error return. This includes sequences such as: <F4 90 80 80>, <C0 80>,
+    or <A0> in UTF-8, and values above 0x10FFFF in UTF-32. Conformant code
+    must check for illegal sequences.
+
+    When the flag is set to lenient, characters over 0x10FFFF are converted
+    to the replacement character; otherwise (when the flag is set to strict)
+    they constitute an error.
+
+    Output parameters:
+	The value "sourceIllegal" is returned from some routines if the input
+	sequence is malformed.  When "sourceIllegal" is returned, the source
+	value will point to the illegal value that caused the problem. E.g.,
+	in UTF-8 when a sequence is malformed, it points to the start of the
+	malformed sequence.  
+
+    Author: Mark E. Davis, 1994.
+    Rev History: Rick McGowan, fixes & updates May 2001.
+		 Fixes & updates, Sept 2001.
+
+------------------------------------------------------------------------ */
+
+/* ---------------------------------------------------------------------
+    The following 4 definitions are compiler-specific.
+    The C standard does not guarantee that wchar_t has at least
+    16 bits, so wchar_t is no less portable than unsigned short!
+    All should be unsigned values to avoid sign extension during
+    bit mask & shift operations.
+------------------------------------------------------------------------ */
+
+typedef unsigned int	UTF32;	/* at least 32 bits */
+typedef unsigned short	UTF16;	/* at least 16 bits */
+typedef unsigned char	UTF8;	/* typically 8 bits */
+typedef unsigned char	Boolean; /* 0 or 1 */
+
+/* Some fundamental constants */
+#define UNI_REPLACEMENT_CHAR (UTF32)0x0000FFFD
+#define UNI_MAX_BMP (UTF32)0x0000FFFF
+#define UNI_MAX_UTF16 (UTF32)0x0010FFFF
+#define UNI_MAX_UTF32 (UTF32)0x7FFFFFFF
+#define UNI_MAX_LEGAL_UTF32 (UTF32)0x0010FFFF
+
+typedef enum {
+	conversionOK, 		/* conversion successful */
+	sourceExhausted,	/* partial character in source, but hit end */
+	targetExhausted,	/* insuff. room in target for conversion */
+	sourceIllegal		/* source sequence is illegal/malformed */
+} ConversionResult;
+
+typedef enum {
+	strictConversion = 0,
+	lenientConversion
+} ConversionFlags;
+
+/* This is for C++ and does no harm in C */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ConversionResult ConvertUTF8toUTF16 (
+		const UTF8** sourceStart, const UTF8* sourceEnd, 
+		UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags);
+
+ConversionResult ConvertUTF16toUTF8 (
+		const UTF16** sourceStart, const UTF16* sourceEnd, 
+		UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags);
+		
+ConversionResult ConvertUTF8toUTF32 (
+		const UTF8** sourceStart, const UTF8* sourceEnd, 
+		UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags);
+
+ConversionResult ConvertUTF32toUTF8 (
+		const UTF32** sourceStart, const UTF32* sourceEnd, 
+		UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags);
+		
+ConversionResult ConvertUTF16toUTF32 (
+		const UTF16** sourceStart, const UTF16* sourceEnd, 
+		UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags);
+
+ConversionResult ConvertUTF32toUTF16 (
+		const UTF32** sourceStart, const UTF32* sourceEnd, 
+		UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags);
+
+Boolean isLegalUTF8Sequence(const UTF8 *source, const UTF8 *sourceEnd);
+
+#ifdef __cplusplus
+}
+#endif
+
+/* --------------------------------------------------------------------- */

corelib/src/DBDriver.cpp

@@ -0,0 +1,774 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/DBDriver.h"
+
+#include "Signature.h"
+#include "VWDictionary.h"
+#include "utilite/UConversion.h"
+#include "utilite/UMath.h"
+#include "utilite/ULogger.h"
+#include "utilite/UTimer.h"
+#include "utilite/UStl.h"
+
+namespace rtabmap {
+
+DBDriver::DBDriver(const ParametersMap & parameters) :
+	_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
+	_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
+	_asyncWaiting(true),
+	_emptyTrashesTime(0)
+{
+	this->parseParameters(parameters);
+}
+
+DBDriver::~DBDriver()
+{
+	this->kill();
+	this->emptyTrashes();
+}
+
+void DBDriver::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kDbMinSignaturesToSave())) != parameters.end())
+	{
+		_minSignaturesToSave = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kDbMinWordsToSave())) != parameters.end())
+	{
+		_minWordsToSave = std::atoi((*iter).second.c_str());
+	}
+}
+
+void DBDriver::closeConnection()
+{
+	this->kill();
+	this->emptyTrashes();
+	_dbSafeAccessMutex.lock();
+	this->disconnectDatabaseQuery();
+	_dbSafeAccessMutex.unlock();
+}
+
+bool DBDriver::openConnection(const std::string & url)
+{
+	_url = url;
+	_dbSafeAccessMutex.lock();
+	if(this->connectDatabaseQuery(url))
+	{
+		this->start();
+		_dbSafeAccessMutex.unlock();
+		return true;
+	}
+	_dbSafeAccessMutex.unlock();
+	return false;
+}
+
+bool DBDriver::isConnected() const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = isConnectedQuery();
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+// In bytes
+long DBDriver::getMemoryUsed() const
+{
+	long bytes;
+	_dbSafeAccessMutex.lock();
+	bytes = getMemoryUsedQuery();
+	_dbSafeAccessMutex.unlock();
+	return bytes;
+}
+
+void DBDriver::mainLoop()
+{
+	UDEBUG("");
+	this->emptyTrashes();
+	this->kill(); // Do it only once
+	UDEBUG("");
+}
+
+void DBDriver::killCleanup()
+{
+	UDEBUG("");
+}
+
+void DBDriver::beginTransaction() const
+{
+	_transactionMutex.lock();
+	ULOGGER_DEBUG("");
+	this->executeNoResultQuery("BEGIN TRANSACTION;");
+}
+
+void DBDriver::commit() const
+{
+	ULOGGER_DEBUG("");
+	this->executeNoResultQuery("COMMIT;");
+	_transactionMutex.unlock();
+}
+
+bool DBDriver::executeNoResult(const std::string & sql) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->executeNoResultQuery(sql);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+void DBDriver::emptyTrashes(bool async)
+{
+	ULOGGER_DEBUG("");
+	if(async)
+	{
+		ULOGGER_DEBUG("Async emptying, start the trash thread");
+		this->start();
+		return;
+	}
+
+	UTimer totalTime;
+	totalTime.start();
+
+	std::vector<Signature*> signatures;
+	std::map<int, VisualWord*> visualWords;
+	_trashesMutex.lock();
+	{
+		signatures = uValues(_trashSignatures);
+		visualWords = _trashVisualWords;
+		_trashSignatures.clear();
+		_trashVisualWords.clear();
+
+		_asyncWaiting = true;
+
+		_dbSafeAccessMutex.lock();
+	}
+	_trashesMutex.unlock();
+
+	if(signatures.size() || visualWords.size())
+	{
+		ULOGGER_DEBUG("trashSignatures size = %d, trashVisualWords size = %d", signatures.size(), visualWords.size());
+		this->beginTransaction();
+		UTimer timer;
+		timer.start();
+		if(signatures.size())
+		{
+			if(this->isConnected())
+			{
+				//Only one query to the database
+				this->saveOrUpdate(signatures);
+			}
+
+			for(std::vector<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+			{
+				delete *iter;
+			}
+			signatures.clear();
+		}
+		ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
+		if(visualWords.size())
+		{
+			if(this->isConnected())
+			{
+				//Only one query to the database
+				this->saveQuery(uValues(visualWords));
+			}
+
+			for(std::map<int, VisualWord *>::iterator iter=visualWords.begin(); iter!=visualWords.end(); ++iter)
+			{
+				delete (*iter).second;
+			}
+			visualWords.clear();
+		}
+		ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
+		this->commit();
+	}
+
+	_emptyTrashesTime = totalTime.ticks();
+	ULOGGER_DEBUG("Total time emptying trashes = %fs...", _emptyTrashesTime);
+
+	_dbSafeAccessMutex.unlock();
+}
+
+void DBDriver::asyncSave(Signature * s)
+{
+	_trashesMutex.lock();
+	{
+		_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
+		if(_trashSignatures.size() > _minSignaturesToSave && this->isRunning() && _asyncWaiting)
+		{
+			ULOGGER_DEBUG("(Sign) Releasing addSem...");
+			_asyncWaiting = false;
+			this->start();
+		}
+	}
+	_trashesMutex.unlock();
+}
+
+void DBDriver::asyncSave(VisualWord * vw)
+{
+	_trashesMutex.lock();
+	{
+		_trashVisualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
+		if(_trashVisualWords.size() > _minWordsToSave && this->isRunning() && _asyncWaiting)
+		{
+			ULOGGER_DEBUG("(Word) Releasing addSem...");
+			_asyncWaiting = false;
+			this->start();
+		}
+	}
+	_trashesMutex.unlock();
+}
+
+bool DBDriver::getSignature(int signatureId, Signature ** s)
+{
+	*s = 0;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::map<int, Signature*>::iterator i=_trashSignatures.begin(); i!=_trashSignatures.end();)
+		{
+			if(i->first == signatureId)
+			{
+				*s = i->second;
+				_trashSignatures.erase(i++);
+				break;
+			}
+			else
+			{
+				++i;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+
+	if(*s == 0)
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadQuery(signatureId, s);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+
+	return true;
+}
+
+bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
+{
+	*vw = 0;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::map<int, VisualWord*>::iterator i=_trashVisualWords.begin(); i!=_trashVisualWords.end(); ++i)
+		{
+			if((*i).first == wordId)
+			{
+				*vw = (*i).second;
+				_trashVisualWords.erase(i);
+				break;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+
+	if(*vw == 0)
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadQuery(wordId, vw);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+
+	return true;
+}
+
+//Automatically begin and commit a transaction
+bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
+{
+	ULOGGER_DEBUG("");
+	std::list<KeypointSignature *> toSaveK;
+	std::list<Signature *> toUpdate;
+	if(this->isConnected() && signatures.size())
+	{
+		for(std::vector<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end();++i)
+		{
+			if((*i)->isSaved())
+			{
+				toUpdate.push_back(*i);
+			}
+			else if((*i)->signatureType().compare("KeypointSignature") == 0)
+			{
+				toSaveK.push_back((KeypointSignature *)(*i));
+			}
+			else
+			{
+				ULOGGER_ERROR("Unknown signature type ?!?");
+			}
+		}
+
+		if(toUpdate.size())
+		{
+			this->updateQuery(toUpdate);
+		}
+		if(toSaveK.size())
+		{
+			this->saveQuery(toSaveK);
+		}
+	}
+	return false;
+}
+
+bool DBDriver::load(VWDictionary * dictionary) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->loadQuery(dictionary);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::loadLastSignatures(std::list<Signature *> & signatures) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->loadLastSignaturesQuery(signatures);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures, bool onlyParents)
+{
+	UDEBUG("");
+	// look up in the trash before the database
+	std::list<int> ids = signIds;
+	std::list<Signature*>::iterator sIter;
+	bool valueFound = false;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
+		{
+			valueFound = false;
+			for(std::map<int, Signature*>::iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end();)
+			{
+				if(sIter->first == *iter)
+				{
+					if((onlyParents && sIter->second->getLoopClosureId() == 0) || !onlyParents)
+					{
+						signatures.push_back(sIter->second);
+						_trashSignatures.erase(sIter++);
+					}
+					else
+					{
+						++sIter;
+					}
+					valueFound = true;
+					break;
+				}
+				else
+				{
+					++sIter;
+				}
+			}
+			if(valueFound)
+			{
+				iter = ids.erase(iter);
+			}
+			else
+			{
+				++iter;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+	UDEBUG("");
+	if(ids.size())
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadKeypointSignaturesQuery(ids, signatures, onlyParents);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	else if(signatures.size())
+	{
+		return true;
+	}
+	return false;
+}
+
+bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws)
+{
+	if(!wordIds.size())
+	{
+		return false;
+	}
+	// look up in the trash before the database
+	std::list<int> ids = wordIds;
+	std::map<int, VisualWord*>::iterator wIter;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
+		{
+			wIter = _trashVisualWords.find(*iter);
+			if(wIter != _trashVisualWords.end())
+			{
+				//UDEBUG("put back word %d from trash", *iter);
+				vws.push_back(wIter->second);
+				_trashVisualWords.erase(wIter);
+				iter = ids.erase(iter);
+			}
+			else
+			{
+				++iter;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+	if(ids.size())
+	{
+		bool r;
+		_dbSafeAccessMutex.lock();
+		r = this->loadWordsQuery(ids, vws);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	else if(vws.size())
+	{
+		return true;
+	}
+	return false;
+}
+
+// <oldWordId, activeWordId>
+bool DBDriver::changeWordsRef(const std::map<int, int> & refsToChange)
+{
+	//Change references in the trash
+	KeypointSignature * s = 0;
+	UTimer timer;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		timer.start();
+		for(std::map<int, Signature *>::iterator iter = _trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
+		{
+			s = dynamic_cast<KeypointSignature*>(iter->second);
+			if(s)
+			{
+				for(std::map<int, int>::const_iterator jter = refsToChange.begin(); jter!=refsToChange.end(); ++jter)
+				{
+					s->changeWordsRef((*jter).first, (*jter).second);
+				}
+			}
+		}
+		ULOGGER_DEBUG("Trash changing words references time=%fs", timer.ticks());
+	}
+	_trashesMutex.unlock();
+
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->changeWordsRefQuery(refsToChange);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::deleteWords(const std::vector<int> & ids)
+{
+	//Delete words in the trash
+	std::map<int, VisualWord*>::iterator iter;
+	_trashesMutex.lock();
+	{
+		_dbSafeAccessMutex.lock();
+		_dbSafeAccessMutex.unlock();
+		for(unsigned int i=0; i<ids.size(); ++i)
+		{
+			iter = _trashVisualWords.find(ids[i]);
+			if(iter != _trashVisualWords.end())
+			{
+				_trashVisualWords.erase(iter);
+				delete (*iter).second;
+			}
+		}
+	}
+	_trashesMutex.unlock();
+
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->deleteWordsQuery(ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::deleteAllVisualWords() const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::string query;
+		query += "DELETE FROM VisualWord;";
+
+		_dbSafeAccessMutex.lock();
+		bool r = this->executeNoResultQuery(query);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::deleteAllObsoleteSSVWLinks() const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::string query;
+		query += "DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT id FROM VisualWord WHERE id = Map_SS_VW.visualWordId);";
+
+		_dbSafeAccessMutex.lock();
+		bool r = this->executeNoResultQuery(query);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::deleteUnreferencedWords() const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::string query = "DELETE FROM visualword WHERE id NOT IN (SELECT visualWordid FROM map_ss_vw);";
+		_dbSafeAccessMutex.lock();
+		bool r = this->executeNoResultQuery(query);
+		_dbSafeAccessMutex.unlock();
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::addNeighbor(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates)
+{
+	bool r = false;
+	Signature * s = 0;
+	_trashesMutex.lock();
+	s = uValue(_trashSignatures, id, s);
+	if(s)
+	{
+		s->addNeighbor(neighbor, actuatorStates);
+		r = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!r)
+	{
+		_dbSafeAccessMutex.lock();
+		r = this->addNeighborQuery(id, neighbor, actuatorStates);
+		_dbSafeAccessMutex.unlock();
+	}
+
+	return r;
+}
+
+bool DBDriver::removeNeighbor(int id, int neighbor)
+{
+	bool r = false;
+	Signature * s = 0;
+	_trashesMutex.lock();
+	s = uValue(_trashSignatures, id, s);
+	if(s)
+	{
+		s->removeNeighbor(neighbor);
+		r = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!r)
+	{
+		r = executeNoResult("DELETE FROM Neighbor WHERE sid=" + uNumber2str(id) + " AND nid=" + uNumber2str(neighbor));
+	}
+
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getImage(int id, IplImage ** img) const
+{
+	CvMat * compressed = 0;
+	_dbSafeAccessMutex.lock();
+	bool result = this->getImageCompressedQuery(id, &compressed);
+	if(compressed)
+	{
+		(*img) = cvDecodeImage(compressed, CV_LOAD_IMAGE_ANYCOLOR);
+		cvReleaseMat(&compressed);
+	}
+	_dbSafeAccessMutex.unlock();
+	return result;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getNeighborIdsQuery(signatureId, neighbors);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::loadNeighbors(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->loadNeighborsQuery(signatureId, neighbors);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getWeight(int signatureId, int & weight) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getWeightQuery(signatureId, weight);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getLoopClosureId(int signatureId, int & loopId) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getLoopClosureIdQuery(signatureId, loopId);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getImageCompressed(int id, CvMat ** compressed) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getImageCompressedQuery(id, compressed);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getAllSignatureIds(std::set<int> & ids) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getAllSignatureIdsQuery(ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getLastSignatureId(int & id) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getLastSignatureIdQuery(id);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getLastVisualWordId(int & id) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getLastVisualWordIdQuery(id);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getSurfNi(int signatureId, int & ni) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getSurfNiQuery(signatureId, ni);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+//TODO Check also in the trash ?
+bool DBDriver::getChildrenIds(int signatureId, std::list<int> & ids) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getChildrenIdsQuery(signatureId, ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const
+{
+	bool r;
+	_dbSafeAccessMutex.lock();
+	r = this->getHighestWeightedSignaturesQuery(count, ids);
+	_dbSafeAccessMutex.unlock();
+	return r;
+}
+
+bool DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::stringstream query;
+		query << "INSERT INTO StatisticsAfterRun(stMemSize,lastSignAdded,processMemUsed,databaseMemUsed) values("
+			  << stMemSize << ","
+		      << lastSignAdded << ","
+		      << processMemUsed << ","
+			  << databaseMemUsed << ");";
+
+		bool r = this->executeNoResultQuery(query.str());
+		return r;
+	}
+	return false;
+}
+
+bool DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
+{
+	ULOGGER_DEBUG("");
+	if(this->isConnected())
+	{
+		std::stringstream query;
+		query << "INSERT INTO StatisticsAfterRunSurf(dictionarySize) values(" << dictionarySize << ");";
+
+		bool r = this->executeNoResultQuery(query.str());
+		return r;
+	}
+	return false;
+}
+
+} // namespace rtabmap

corelib/src/DBDriverFactory.cpp

@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/DBDriverFactory.h"
+#include "DBDriverSqlite3.h"
+#include "utilite/ULogger.h"
+
+namespace rtabmap {
+
+DBDriver * DBDriverFactory::createDBDriver(const std::string & dbDriverName, const ParametersMap & parameters)
+{
+	// TODO Do it with dynamic link libraries...
+	// Find the driver...
+	// Link dynamically to the driver...
+
+	DBDriver * driver = 0;
+
+	// Static link
+	if(dbDriverName.compare("sqlite3") == 0)
+	{
+		driver = new DBDriverSqlite3(parameters);
+	}
+	else if(dbDriverName.compare("mysql") == 0)
+	{
+		// TODO mysql driver
+		ULOGGER_ERROR("mysql driver is not implemented!");
+	}
+	else if(dbDriverName.compare("postgresql") == 0)
+	{
+		// TODO postgresql driver
+		ULOGGER_ERROR("postgresql driver is not implemented!");
+	}
+	else if(dbDriverName.compare("oracle") == 0)
+	{
+		// TODO oracle driver
+		ULOGGER_ERROR("oracle driver is not implemented!");
+	}
+	else
+	{
+		ULOGGER_ERROR("Unknown driver \"%s\"", dbDriverName.c_str());
+	}
+
+	return driver;
+}
+
+DBDriverFactory::DBDriverFactory() {
+
+}
+
+DBDriverFactory::~DBDriverFactory() {
+}
+
+}
+

corelib/src/DBDriverSqlite3.h

@@ -0,0 +1,90 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef DBDRIVERSQLITE3_H_
+#define DBDRIVERSQLITE3_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include "rtabmap/core/DBDriver.h"
+
+#include <sqlite3.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP DBDriverSqlite3: public DBDriver {
+public:
+	DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
+	virtual ~DBDriverSqlite3();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	void setDbInMemory(bool dbInMemory);
+	void setJournalMode(int journalMode);
+	void setCacheSize(unsigned int cacheSize);
+
+private:
+	virtual bool connectDatabaseQuery(const std::string & url);
+	virtual void disconnectDatabaseQuery();
+	virtual bool isConnectedQuery() const;
+	virtual long getMemoryUsedQuery() const; // In bytes
+
+	virtual bool executeNoResultQuery(const std::string & sql) const;
+
+	virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const; // <oldWordId, activeWordId>
+	virtual bool deleteWordsQuery(const std::vector<int> & ids) const;
+	virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors) const;
+	virtual bool getWeightQuery(int signatureId, int & weight) const;
+	virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const;
+	virtual bool addNeighborQuery(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) const;
+
+	virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const;
+	virtual bool updateQuery(const std::list<Signature *> & signatures) const;
+	virtual bool saveQuery(const KeypointSignature * ss) const;
+	virtual bool saveQuery(const std::list<KeypointSignature *> & signatures) const;
+
+	// Load objects
+	virtual bool loadQuery(VWDictionary * dictionary) const;
+	virtual bool loadLastSignaturesQuery(std::list<Signature *> & signatures) const;
+	virtual bool loadQuery(int signatureId, Signature ** s) const;
+	virtual bool loadQuery(int wordId, VisualWord ** vw) const;
+	virtual bool loadQuery(int signatureId, KeypointSignature * ss) const;
+	virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool onlyParents = false) const;
+	virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const;
+	virtual bool loadNeighborsQuery(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const;
+
+	virtual bool getImageCompressedQuery(int id, CvMat ** compressed) const;
+	virtual bool getAllSignatureIdsQuery(std::set<int> & ids) const;
+	virtual bool getLastSignatureIdQuery(int & id) const;
+	virtual bool getLastVisualWordIdQuery(int & id) const;
+	virtual bool getSurfNiQuery(int signatureId, int & ni) const;
+	virtual bool getChildrenIdsQuery(int signatureId, std::list<int> & ids) const;
+	virtual bool getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int, int> & ids) const;
+
+private:
+	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
+
+private:
+	sqlite3 * _ppDb;
+	bool _dbInMemory;
+	unsigned int _cacheSize;
+	int _journalMode;
+};
+
+}
+
+#endif /* DBDRIVERSQLITE3_H_ */

corelib/src/EpipolarGeometry.cpp

@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/EpipolarGeometry.h"
+#include "utilite/ULogger.h"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/core_c.h>
+
+namespace rtabmap
+{
+
+//Epipolar geometry
+void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
+{
+	if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
+	{
+		ULOGGER_ERROR("The matrix is not the good size...");
+		return;
+	}
+
+	if(fundamentalMatrix.type() != CV_64FC1)
+	{
+		ULOGGER_ERROR("The matrix is not the good type...");
+		return;
+	}
+
+	CvMat * w = cvCreateMat(3, 3, CV_64FC1);
+	CvMat * u = cvCreateMat(3, 3, CV_64FC1);
+	CvMat * v = cvCreateMat(3, 3, CV_64FC1);
+
+	CvMat f = fundamentalMatrix;
+	cvSVD(&f, w, u, v);
+
+	// v is for image 1
+	// u is for image 2
+
+	e1[0] = v->data.db[0*3+2];// /v->data.db[2*3+2];
+	e1[1] = v->data.db[1*3+2];// /v->data.db[2*3+2];
+	e1[2] = v->data.db[2*3+2];// /v->data.db[2*3+2];
+
+	e2[0] = u->data.db[0*3+2];// /u->data.db[2*3+2];
+	e2[1] = u->data.db[1*3+2];// /u->data.db[2*3+2];
+	e2[2] = u->data.db[2*3+2];// /u->data.db[2*3+2];
+
+	cvReleaseMat(&w);
+	cvReleaseMat(&u);
+	cvReleaseMat(&v);
+}
+
+// P2 = [M | t] = [[e']_x * F | e']
+void findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2)
+{
+	if(p2.rows != 3 || p2.cols != 4 || fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
+	{
+		ULOGGER_ERROR("Matrices are not the good size... ");
+		return;
+	}
+
+	if(p2.type()!= CV_64FC1 || fundamentalMatrix.type() != CV_64FC1)
+	{
+		ULOGGER_ERROR("Matrices are not the good type...");
+		return;
+	}
+
+	if(e2[0] == 0 && e2[1] == 0 && e2[2] == 0)
+	{
+		cv::Vec3d e1;
+		findEpipolesFromF(fundamentalMatrix, e1, e2);
+	}
+
+	double e2_sd[3*3] = { 0., -e2[2], e2[1],
+						   e2[2], 0., -e2[0],
+						  -e2[1], e2[0], 0. };
+	CvMat e2_smt = cvMat( 3, 3, CV_64FC1, e2_sd );
+	cv::Mat e2_sm(&e2_smt); //;
+
+	cv::Mat m = e2_sm*fundamentalMatrix;
+
+	p2.at<double>(0,0) = m.at<double>(0,0);
+	p2.at<double>(0,1) = m.at<double>(0,1);
+	p2.at<double>(0,2) = m.at<double>(0,2);
+	p2.at<double>(1,0) = m.at<double>(1,0);
+	p2.at<double>(1,1) = m.at<double>(1,1);
+	p2.at<double>(1,2) = m.at<double>(1,2);
+	p2.at<double>(2,0) = m.at<double>(2,0);
+	p2.at<double>(2,1) = m.at<double>(2,1);
+	p2.at<double>(2,2) = m.at<double>(2,2);
+
+	p2.at<double>(0,3) = e2[0];
+	p2.at<double>(1,3) = e2[1];
+	p2.at<double>(2,3) = e2[2];
+}
+
+} // namespace rtabmap

corelib/src/KeypointDescriptor.cpp

@@ -0,0 +1,541 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/KeypointDescriptor.h"
+#include "utilite/UStl.h"
+#include "utilite/UConversion.h"
+#include "utilite/ULogger.h"
+#include "utilite/UMath.h"
+#include "utilite/ULogger.h"
+#include <opencv2/imgproc/imgproc_c.h>
+#include <opencv2/gpu/gpu.hpp>
+#include <opencv2/core/version.hpp>
+
+#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
+
+namespace rtabmap {
+
+KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	_childDescriptor(childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+KeypointDescriptor::~KeypointDescriptor()
+{
+	if(_childDescriptor)
+	{
+		delete _childDescriptor;
+	}
+}
+
+void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	if(_childDescriptor)
+	{
+		_childDescriptor->parseParameters(parameters);
+	}
+}
+
+std::list<std::vector<float> > KeypointDescriptor::generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	// see decorator pattern...
+	std::list<std::vector<float> > descriptors = this->_generateDescriptors(image, keypoints);
+	std::list<std::vector<float> > childDescriptors;
+	if(_childDescriptor)
+	{
+		childDescriptors = _childDescriptor->generateDescriptors(image, keypoints);
+
+		if(childDescriptors.size() && childDescriptors.size() == descriptors.size())
+		{
+			std::list<std::vector<float> >::iterator iterDesc = descriptors.begin();
+			std::list<std::vector<float> >::iterator iterChild = childDescriptors.begin();
+			for(; iterDesc!=descriptors.end(); ++iterDesc, ++iterChild)
+			{
+				iterDesc->insert(iterDesc->end(), iterChild->begin(), iterChild->end());
+			}
+		}
+	}
+	return descriptors;
+}
+
+void KeypointDescriptor::setChildDescriptor(KeypointDescriptor * childDescriptor)
+{
+	if(_childDescriptor)
+	{
+		delete _childDescriptor;
+	}
+	_childDescriptor = childDescriptor;
+}
+
+//////////////////////////
+//SURFDescriptor
+//////////////////////////
+SURFDescriptor::SURFDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
+	_surf.extended = Parameters::defaultSURFExtended();
+	_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
+	_surf.nOctaves = Parameters::defaultSURFOctaves();
+	_gpuVersion = Parameters::defaultSURFGpuVersion();
+	_upright = Parameters::defaultSURFUpright();
+	this->parseParameters(parameters);
+}
+
+SURFDescriptor::~SURFDescriptor()
+{
+}
+
+void SURFDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
+	{
+		_surf.extended = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
+	{
+		_surf.hessianThreshold = std::atof((*iter).second.c_str()); // is it needed for the descriptor?
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
+	{
+		_surf.nOctaveLayers = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
+	{
+		_surf.nOctaves = std::atoi((*iter).second.c_str()); // is it needed for the descriptor?
+	}
+	if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
+	{
+		_gpuVersion = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
+	{
+		_upright = uStr2Bool((*iter).second.c_str());
+	}
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > SURFDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+	cv::Mat mask;
+	std::vector<cv::KeyPoint> k = uListToVector(keypoints);
+	std::vector<float> d;
+#if OPENCV_SURF_GPU
+	if(_gpuVersion)
+	{
+		cv::gpu::GpuMat imgGpu(img);
+		cv::gpu::GpuMat descriptorsGpu;
+		cv::gpu::GpuMat keypointsGpu;
+		cv::gpu::SURF_GPU surfGpu(_surf.hessianThreshold, _surf.nOctaves, _surf.nOctaveLayers, _surf.extended, 0.01f, _upright);
+		surfGpu.uploadKeypoints(k, keypointsGpu);
+		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
+		surfGpu.downloadDescriptors(descriptorsGpu, d);
+	}
+	else
+	{
+		_surf(img, mask, k, d, true); // Opencv surf descriptors
+	}
+#else
+	_surf(img, mask, k, d, true); // Opencv surf descriptors
+#endif
+
+	unsigned int dim = _surf.descriptorSize();
+	for(unsigned int i=0; i<d.size(); i+=dim)
+	{
+		descriptors.push_back(std::vector<float>(d.begin()+i, d.begin()+i+dim));
+	}
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return descriptors;
+}
+
+//////////////////////////
+//SIFTDescriptor
+//////////////////////////
+SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+SIFTDescriptor::~SIFTDescriptor()
+{
+}
+
+void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > SIFTDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+	cv::Mat mask;
+	std::vector<cv::KeyPoint> k = uListToVector(keypoints);
+	cv::Mat d;
+	cv::SIFT sift(_commonParams, cv::SIFT::DetectorParams(), _descriptorParams);
+	sift(img, mask, k, d, true); // Opencv surf descriptors
+	unsigned int dim = sift.descriptorSize();
+	//ULOGGER_DEBUG("row=%d, col=%d, type=%d (float=%d)", d.rows, d.cols, d.type(), CV_32F);
+	for(int i=0; i<d.rows; ++i)
+	{
+		descriptors.push_back(std::vector<float>(d.ptr<float>(i), d.ptr<float>(i)+dim));
+	}
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return descriptors;
+}
+
+//////////////////////////
+//LaplacianDescriptor
+//////////////////////////
+LaplacianDescriptor::LaplacianDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+LaplacianDescriptor::~LaplacianDescriptor()
+{
+}
+
+void LaplacianDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	// No parameter...
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > LaplacianDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+
+	//create descriptors...
+	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	{
+		std::vector<float> laplacian(1);
+		laplacian[0] = uSign(key->response);
+		descriptors.push_back(laplacian);
+	}
+	return descriptors;
+}
+
+//////////////////////////
+//ColorDescriptor
+//////////////////////////
+ColorDescriptor::ColorDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+	KeypointDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+ColorDescriptor::~ColorDescriptor()
+{
+}
+
+void ColorDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	// No parameter...
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > ColorDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+
+	IplImage * imageConverted = 0;
+	if(image->nChannels != 3 || image->depth != IPL_DEPTH_8U)
+	{
+		imageConverted = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 3);
+		cvCvtColor(image, imageConverted, CV_GRAY2BGR);
+	}
+	cv::Mat imgMat;
+	if(imageConverted)
+	{
+		imgMat = cv::Mat(imageConverted);
+	}
+	else
+	{
+		imgMat =  cv::Mat(image);
+	}
+
+	//create descriptors...
+	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	{
+
+		int grayMax = -1;  // grayValue
+		int grayMin = -1;  // grayValue
+		float d[6] = {0};
+		std::vector<int> RxV;
+		cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
+		int R = cvRound(key->size*1.2/9.*2);
+		this->getCircularROI(R, RxV);
+		cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
+		// find the brighter and darker pixels
+		for( int dy = -R; dy <= R; ++dy )
+		{
+			int Rx = RxV[abs(dy)];
+			for( int dx = -Rx; dx <= Rx; ++dx )
+			{
+				if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
+				{
+					//bgr
+					uchar b = img(center.y+dy, center.x+dx)[0];
+					uchar g = img(center.y+dy, center.x+dx)[1];
+					uchar r = img(center.y+dy, center.x+dx)[2];
+					int gray = b*0.114 + g*0.587 + r*0.299;
+					if(grayMax<0 || gray > grayMax)
+					{
+						grayMax = gray;
+						d[0] = b;
+						d[1] = g;
+						d[2] = r;
+					}
+					if(grayMin<0 || gray < grayMin)
+					{
+						grayMin = gray;
+						d[3] = b;
+						d[4] = g;
+						d[5] = r;
+					}
+				}
+				else
+				{
+					//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
+				}
+			}
+		}
+		for(int i=0; i<6; ++i)
+		{
+			d[i] /= 255; // Normalize between 0 and 1
+		}
+		descriptors.push_back(std::vector<float>(d, d + sizeof(d) / sizeof(float)));
+	}
+
+	if(imageConverted)
+	{
+		cvReleaseImage(&imageConverted);
+	}
+
+	return descriptors;
+}
+
+// the function returns x boundary coordinates of
+// the circle for each y. RxV[y1] = x1 means that
+// when y=y1, -x1 <=x<=x1 is inside the circle
+// (from OpenCv doc, C++ Cheatsheet)
+void ColorDescriptor::getCircularROI(int R, std::vector<int> & RxV) const
+{
+    RxV.resize(R+1);
+    for( int y = 0; y <= R; y++ )
+        RxV[y] = cvRound(sqrt(double(R*R - y*y)));
+}
+
+//////////////////////////
+//HueDescriptor
+//////////////////////////
+HueDescriptor::HueDescriptor(const ParametersMap & parameters, KeypointDescriptor * childDescriptor) :
+		ColorDescriptor(parameters, childDescriptor)
+{
+	this->parseParameters(parameters);
+}
+
+HueDescriptor::~HueDescriptor()
+{
+}
+
+void HueDescriptor::parseParameters(const ParametersMap & parameters)
+{
+	// No parameter...
+	KeypointDescriptor::parseParameters(parameters);
+}
+
+std::list<std::vector<float> > HueDescriptor::_generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const
+{
+	ULOGGER_DEBUG("");
+	std::list<std::vector<float> > descriptors;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+
+	IplImage * imageConverted = 0;
+	if(image->nChannels != 3 || image->depth != IPL_DEPTH_8U)
+	{
+		imageConverted = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 3);
+		cvCvtColor(image, imageConverted, CV_GRAY2BGR);
+	}
+	cv::Mat imgMat;
+	if(imageConverted)
+	{
+		imgMat = cv::Mat(imageConverted);
+	}
+	else
+	{
+		imgMat =  cv::Mat(image);
+	}
+
+	//create descriptors...
+	for(std::list<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
+	{
+
+		int intensityMax = -1;
+		int intensityMin = -1;
+		float d[2] = {0};
+		std::vector<int> RxV;
+		cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
+		int R = cvRound(key->size*1.2/9.*2);
+		this->getCircularROI(R, RxV);
+		cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
+		// find the brighter and darker pixels using the intensity
+		int dxb=0;
+		int dyb=0;
+		int dxd=0;
+		int dyd=0;
+		for( int dy = -R; dy <= R; ++dy )
+		{
+			int Rx = RxV[abs(dy)];
+			for( int dx = -Rx; dx <= Rx; ++dx )
+			{
+				if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
+				{
+					//bgr
+					float b = float(img(center.y+dy, center.x+dx)[0]) / 255.0f;
+					float g = float(img(center.y+dy, center.x+dx)[1]) / 255.0f;
+					float r = float(img(center.y+dy, center.x+dx)[2]) / 255.0f;
+					int intensity = rgb2intensity(r, g, b);
+					if(intensityMax<0 || intensity > intensityMax)
+					{
+						intensityMax = intensity;
+						dxb = dx;
+						dyb = dy;
+					}
+					if(intensityMin<0 || intensity < intensityMin)
+					{
+						intensityMin = intensity;
+						dxd = dx;
+						dyd = dy;
+					}
+				}
+				else
+				{
+					//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
+				}
+			}
+		}
+
+		// brighter
+		float b = float(img(center.y+dyb, center.x+dxb)[0]) / 255.0f;
+		float g = float(img(center.y+dyb, center.x+dxb)[1]) / 255.0f;
+		float r = float(img(center.y+dyb, center.x+dxb)[2]) / 255.0f;
+		d[0] = rgb2hue(r, g, b);
+
+		// darker
+		b = float(img(center.y+dyd, center.x+dxd)[0]) / 255.0f;
+		g = float(img(center.y+dyd, center.x+dxd)[1]) / 255.0f;
+		r = float(img(center.y+dyd, center.x+dxd)[2]) / 255.0f;
+		d[1] = rgb2hue(r, g, b);
+
+		descriptors.push_back(std::vector<float>(d, d + sizeof(d) / sizeof(float)));
+	}
+
+	if(imageConverted)
+	{
+		cvReleaseImage(&imageConverted);
+	}
+
+	return descriptors;
+}
+
+// assuming that rgb values are normalized [0,1]
+float HueDescriptor::rgb2hue(float r, float g, float b) const
+{
+	double pi = 3.14159265359;
+	if(b<=g)
+	{
+		return acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b))))/pi;
+	}
+	else
+	{
+		return (pi-acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b)))))/pi;
+	}
+}
+
+
+}

corelib/src/KeypointDetector.cpp

@@ -0,0 +1,496 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/KeypointDetector.h"
+#include "VWDictionary.h"
+#include "utilite/ULogger.h"
+#include "utilite/UTimer.h"
+#include "utilite/UStl.h"
+#include "rtabmap/core/Parameters.h"
+#include "utilite/UConversion.h"
+#include <opencv2/imgproc/imgproc_c.h>
+#include <opencv2/gpu/gpu.hpp>
+#include <opencv2/core/version.hpp>
+
+#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
+
+namespace rtabmap
+{
+
+KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
+		_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
+		_usingAdaptiveResponseThr(Parameters::defaultKpUsingAdaptiveResponseThr()),
+		_adaptiveResponseThr(1),
+		_roiRatios(std::vector<float>(4, 0.0f))
+{
+	this->setRoi(Parameters::defaultKpRoiRatios());
+	this->parseParameters(parameters);
+}
+
+void KeypointDetector::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
+	{
+		_wordsPerImageTarget = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kKpUsingAdaptiveResponseThr())) != parameters.end())
+	{
+		_usingAdaptiveResponseThr = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
+	{
+		this->setRoi((*iter).second);
+	}
+}
+
+std::list<cv::KeyPoint> KeypointDetector::generateKeypoints(const IplImage * image)
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(image)
+	{
+		UTimer timer;
+		timer.start();
+
+		cv::Rect roi = computeRoi(image);
+
+		// Get keypoints
+		keypoints = this->_generateKeypoints(image, roi);
+		ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
+
+		//clip the number of words... to _wordsPerImageTarget
+		// Variable hessian threshold
+		if(_wordsPerImageTarget > 0)
+		{
+			ULOGGER_DEBUG("_adaptiveResponseThr=%f", _adaptiveResponseThr);
+			if(keypoints.size() > 0)
+			{
+				if(keypoints.size() > _wordsPerImageTarget)
+				{
+					_adaptiveResponseThr *= 1+((float(keypoints.size())/float(_wordsPerImageTarget)-1)/1000);
+				}
+				else if(keypoints.size() < _wordsPerImageTarget)
+				{
+					_adaptiveResponseThr *= 1-((1-float(keypoints.size())/float(_wordsPerImageTarget))/1);
+				}
+				// 10% margin...
+				if(keypoints.size() > 1.1 * _wordsPerImageTarget)
+				{
+					ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
+					// Remove words under the new hessian threshold
+
+					// Sort words by hessian
+					std::multimap<float, std::list<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
+					for(std::list<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
+					{
+						//Keep track of the data, to be easier to manage the data in the next step
+						hessianMap.insert(std::pair<float, std::list<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
+					}
+
+					// Remove them from the signature
+					int removed = 0;
+					unsigned int stopIndex = hessianMap.size()-_wordsPerImageTarget;
+					std::multimap<float, std::list<cv::KeyPoint>::iterator>::iterator iter = hessianMap.begin();
+					for(unsigned int k=0; k < stopIndex && iter!=hessianMap.end(); ++k, ++iter)
+					{
+						keypoints.erase(iter->second);
+						++removed;
+					}
+					if(iter->first!=0)
+					{
+						_adaptiveResponseThr = iter->first;
+					}
+					ULOGGER_DEBUG("%d keypoints removed, (kept %d)", removed, keypoints.size());
+				}
+			}
+			else
+			{
+				_adaptiveResponseThr /= 2;
+			}
+
+			if(_adaptiveResponseThr < this->getMinimumResponseThr())
+			{
+				_adaptiveResponseThr = this->getMinimumResponseThr();
+			}
+
+			ULOGGER_DEBUG("new _adaptiveResponseThr=%f", _adaptiveResponseThr);
+			ULOGGER_DEBUG("adjusting hessian threshold time = %f s", timer.ticks());
+		}
+
+		// Adjust keypoint position to raw image
+		for(std::list<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
+		{
+			iter->pt.x += roi.x;
+			iter->pt.y += roi.y;
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("Image is null!");
+	}
+	return keypoints;
+}
+
+void KeypointDetector::setRoi(const std::string & roi)
+{
+	std::list<std::string> strValues = uSplit(roi, ' ');
+	if(strValues.size() != 4)
+	{
+		ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
+	}
+	else
+	{
+		std::vector<float> tmpValues(4);
+		unsigned int i=0;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			tmpValues[i] = std::atof((*iter).c_str());
+			++i;
+		}
+
+		if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+			tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+			tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+			tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+		{
+			_roiRatios = tmpValues;
+		}
+		else
+		{
+			ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
+		}
+	}
+}
+
+cv::Rect KeypointDetector::computeRoi(const IplImage * image) const
+{
+	if(image && _roiRatios.size() == 4)
+	{
+		cv::Rect roi(0, 0, image->width, image->height);
+		UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
+		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
+
+		float width = image->width;
+		float height = image->height;
+		//left roi
+		if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
+		{
+			roi.x = width * _roiRatios[0];
+		}
+
+		//right roi
+		roi.width = width - roi.x;
+		if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
+		{
+			roi.width -= width * _roiRatios[1];
+		}
+
+		//top roi
+		if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
+		{
+			roi.y = height * _roiRatios[2];
+		}
+
+		//bottom roi
+		roi.height = height - roi.y;
+		if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
+		{
+			roi.height -= height * _roiRatios[3];
+		}
+		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
+
+		return roi;
+	}
+	else
+	{
+		UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
+		return cv::Rect();
+	}
+}
+
+
+//////////////////////////
+//SURFDetector
+//////////////////////////
+SURFDetector::SURFDetector(const ParametersMap & parameters) :
+		KeypointDetector(parameters)
+{
+	_surf.hessianThreshold = Parameters::defaultSURFHessianThreshold();
+	_surf.extended = Parameters::defaultSURFExtended();
+	_surf.nOctaveLayers = Parameters::defaultSURFOctaveLayers();
+	_surf.nOctaves = Parameters::defaultSURFOctaves();
+	_gpuVersion = Parameters::defaultSURFGpuVersion();
+	_upright = Parameters::defaultSURFUpright();
+	this->parseParameters(parameters);
+	this->setAdaptiveResponseThr(_surf.hessianThreshold);
+}
+
+SURFDetector::~SURFDetector()
+{
+}
+
+void SURFDetector::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
+	{
+		_surf.extended = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
+	{
+		_surf.hessianThreshold = std::atof((*iter).second.c_str());
+		this->setAdaptiveResponseThr(_surf.hessianThreshold);
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
+	{
+		_surf.nOctaveLayers = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
+	{
+		_surf.nOctaves = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
+	{
+		_surf.nOctaves = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
+	{
+		_gpuVersion = uStr2Bool((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
+	{
+		_upright = uStr2Bool((*iter).second.c_str());
+	}
+	KeypointDetector::parseParameters(parameters);
+}
+
+std::list<cv::KeyPoint> SURFDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return keypoints;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+
+	cv::SURF surf = _surf;
+	if(this->isUsingAdaptiveResponseThr())
+	{
+		surf.hessianThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+	}
+
+	cv::Mat imgRoi(img, roi);
+	std::vector<cv::KeyPoint> k;
+#if OPENCV_SURF_GPU
+	if(_gpuVersion )
+	{
+		cv::gpu::GpuMat imgGpu(imgRoi);
+		cv::gpu::GpuMat keypointsGpu;
+		cv::gpu::SURF_GPU surfGpu(surf.hessianThreshold, surf.nOctaves, surf.nOctaveLayers, surf.extended, 0.01f, _upright);
+		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
+		surfGpu.downloadKeypoints(keypointsGpu, k);
+	}
+	else
+	{
+		surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
+	}
+#else
+	surf(imgRoi, cv::Mat(), k); // Opencv surf keypoints
+#endif
+	keypoints = uVectorToList(k);
+
+
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return keypoints;
+}
+
+//////////////////////////
+//SIFTDetector
+//////////////////////////
+SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
+		KeypointDetector(parameters)
+{
+	_detectorParams.threshold = Parameters::defaultSIFTThreshold();
+	_detectorParams.edgeThreshold = Parameters::defaultSIFTEdgeThreshold();
+	this->parseParameters(parameters);
+	this->setAdaptiveResponseThr(_detectorParams.threshold);
+}
+
+SIFTDetector::~SIFTDetector()
+{
+}
+
+void SIFTDetector::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kSIFTThreshold())) != parameters.end())
+	{
+		_detectorParams.threshold = std::atof((*iter).second.c_str());
+		this->setAdaptiveResponseThr(_detectorParams.threshold);
+	}
+	if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
+	{
+		_detectorParams.edgeThreshold = std::atof((*iter).second.c_str());
+	}
+	KeypointDetector::parseParameters(parameters);
+}
+
+std::list<cv::KeyPoint> SIFTDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return keypoints;
+	}
+	// SURF support only grayscale images
+	IplImage * imageGrayScale = 0;
+	if(image->nChannels != 1 || image->depth != IPL_DEPTH_8U)
+	{
+		imageGrayScale = cvCreateImage(cvSize(image->width,image->height), IPL_DEPTH_8U, 1);
+		cvCvtColor(image, imageGrayScale, CV_BGR2GRAY);
+	}
+	cv::Mat img;
+	if(imageGrayScale)
+	{
+		img = cv::Mat(imageGrayScale);
+	}
+	else
+	{
+		img =  cv::Mat(image);
+	}
+
+	cv::SIFT::DetectorParams detectorParam = _detectorParams;
+	if(this->isUsingAdaptiveResponseThr())
+	{
+		detectorParam.threshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+	}
+	cv::Mat mask;
+	cv::SIFT sift(_commonParams, detectorParam);
+
+	cv::Mat imgRoi(img, roi);
+	std::vector<cv::KeyPoint> k;
+	sift(imgRoi, mask, k); // Opencv surf keypoints
+	keypoints = uVectorToList(k);
+	if(imageGrayScale)
+	{
+		cvReleaseImage(&imageGrayScale);
+	}
+	return keypoints;
+}
+
+
+//////////////////////////
+//StarDetector
+//////////////////////////
+StarDetector::StarDetector(const ParametersMap & parameters) :
+	KeypointDetector(parameters)
+{
+	_star.lineThresholdBinarized = Parameters::defaultStarLineThresholdBinarized();
+	_star.lineThresholdProjected = Parameters::defaultStarLineThresholdProjected();
+	_star.maxSize = Parameters::defaultStarMaxSize();
+	_star.responseThreshold = Parameters::defaultStarResponseThreshold();
+	_star.suppressNonmaxSize = Parameters::defaultStarSuppressNonmaxSize();
+	this->parseParameters(parameters);
+	this->setAdaptiveResponseThr(_star.responseThreshold);
+}
+
+StarDetector::~StarDetector()
+{
+}
+
+void StarDetector::parseParameters(const ParametersMap & parameters)
+{
+	ULOGGER_WARN("The StarDetector parameters can't be changed on ROS (this is an issue with the default (and too old) opencv revision used in ROS)");
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kStarLineThresholdBinarized())) != parameters.end())
+	{
+		_star.lineThresholdBinarized = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kStarLineThresholdProjected())) != parameters.end())
+	{
+		_star.lineThresholdProjected = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kStarMaxSize())) != parameters.end())
+	{
+		_star.maxSize = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kStarResponseThreshold())) != parameters.end())
+	{
+		_star.responseThreshold = int(std::atof((*iter).second.c_str()));
+		this->setAdaptiveResponseThr(_star.responseThreshold);
+	}
+	if((iter=parameters.find(Parameters::kStarSuppressNonmaxSize())) != parameters.end())
+	{
+		_star.suppressNonmaxSize = std::atoi((*iter).second.c_str());
+	}
+	KeypointDetector::parseParameters(parameters);
+}
+
+std::list<cv::KeyPoint> StarDetector::_generateKeypoints(const IplImage * image, const cv::Rect & roi) const
+{
+	ULOGGER_DEBUG("");
+	std::list<cv::KeyPoint> keypoints;
+	if(!image)
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return keypoints;
+	}
+
+	cv::Mat img(image);
+	cv::Mat mask;
+	// TODO More testing needed with the star detector, NN search distance must be changed to 0.8
+	//find keypoints with the star detector
+	cv::StarDetector star = _star;
+	if(this->isUsingAdaptiveResponseThr())
+	{
+		star.responseThreshold = this->getAdaptiveResponseThr(); // use the adaptive threshold
+	}
+
+	// Get keypoints with the star detector
+	cv::Mat imgRoi(img, roi);
+	std::vector<cv::KeyPoint> k;
+	star(imgRoi, k);
+	keypoints = uVectorToList(k);
+	return keypoints;
+}
+
+}

corelib/src/KeypointMemory.h

@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef KEYPOINTMEMORY_H_
+#define KEYPOINTMEMORY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "Memory.h"
+
+namespace rtabmap {
+
+class VWDictionary;
+class VisualWord;
+class KeypointDetector;
+class KeypointDescriptor;
+
+class RTABMAP_EXP KeypointMemory : public Memory
+{
+public:
+	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorUndef};
+	enum DescriptorStrategy {kDescriptorSurf, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef};
+
+public:
+	KeypointMemory(const ParametersMap & parameters = ParametersMap());
+	virtual ~KeypointMemory();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
+	virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::set<int> & signatureIds = std::set<int>()) const;
+	virtual int forget(const std::list<int> & ignoredIds = std::list<int>());
+	virtual int reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched);
+	virtual void dumpMemory(std::string directory) const;
+	virtual void dumpSignatures(const char * fileNameSign) const;
+
+	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+
+	const KeypointDetector * getKeypointDetector() const {return _keypointDetector;}
+	const KeypointDescriptor * getKeypointDescriptor() const {return _keypointDescriptor;}
+	const VWDictionary * getVWD() const {return _vwd;}
+	DetectorStrategy detectorStrategy() const;
+
+protected:
+	virtual Signature * getSignatureLtMem(int id);
+	virtual void addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >());
+	virtual void clear();
+	virtual void moveToTrash(Signature * s);
+	virtual void preUpdate();
+	virtual void merge(const Signature * from, Signature * to, MergingStrategy s);
+
+private:
+	virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false);
+	void disableWordsRef(int signatureId);
+	void enableWordsRef(const std::list<int> & signatureIds);
+	void cleanUnusedWords();
+	int getNi(int signatureId) const;
+
+private:
+	std::list<int> _commonWords;
+	VWDictionary * _vwd;
+	KeypointDetector * _keypointDetector;
+	KeypointDescriptor * _keypointDescriptor;
+	//std::map<int, int> _wordRefsToChange;
+	bool _reactivatedWordsComparedToNewWords;
+	float _badSignRatio;;
+	bool _tfIdfLikelihoodUsed;
+	bool _parallelized;
+	bool _tfIdfNormalized;
+};
+
+}
+
+#endif /* KEYPOINTMEMORY_H_ */

corelib/src/Memory.h

@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef MEMORY_H_
+#define MEMORY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "utilite/UEventsHandler.h"
+#include "rtabmap/core/Parameters.h"
+#include "utilite/UVariant.h"
+#include <typeinfo>
+#include <list>
+#include <map>
+#include <set>
+#include "utilite/UStl.h"
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class Signature;
+class DBDriver;
+class Node;
+class SMState;
+
+class RTABMAP_EXP Memory
+{
+public:
+	static const int kIdStart;
+	static const int kIdVirtual;
+	static const int kIdInvalid;
+
+	enum MergingStrategy{kFullMerging, kUseOnlyFromMerging, kUseOnlyDestMerging};
+
+public:
+	Memory(const ParametersMap & parameters = ParametersMap());
+	virtual ~Memory();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	bool update(const SMState * rawData, std::list<std::pair<std::string, float> > & stats);
+	virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
+	virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::set<int> & signatureIds = std::set<int>()) const;
+	virtual int forget(const std::list<int> & ignoredIds = std::list<int>());
+	virtual int reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, unsigned int maxTouched);
+
+	int cleanup(const std::list<int> & ignoredIds = std::list<int>());
+	void emptyTrash();
+	void joinTrashThread();
+	void addLoopClosureLink(int oldId, int newId, bool rehearsal = false);
+	void getNeighborsId(std::map<int,int> & ids, int signatureId, unsigned int margin, bool checkInDatabase = true, int ignoredId = 0) const;
+
+	//getters
+	unsigned int getWorkingMemSize() const {return _workingMem.size();}
+	unsigned int getStMemSize() const {return _stMem.size();};
+	const std::map<int, int> & getWorkingMem() const {return _workingMem;}
+	const std::set<int> & getStMem() const {return _stMem;}
+	std::list<int> getChildrenIds(int signatureId) const;
+	bool isRawDataKept() const {return _rawDataKept;}
+	std::map<int, int> getWeights() const;
+	int getWeight(int id) const;
+	float getSimilarityOnlyLast() const {return _similarityOnlyLast;}
+	const Signature * getLastSignature() const;
+	int getDatabaseMemoryUsed() const; // in bytes
+	double getDbSavingTime() const;
+	IplImage * getImage(int id) const;
+	bool isDatabaseCleaned()  const {return _databaseCleaned;}
+	bool isCommonSignatureUsed() const {return _commonSignatureUsed;}
+	std::set<int> getAllSignatureIds() const;
+	bool memoryChanged() const {return _memoryChanged;}
+	const Signature * getSignature(int id) const;
+	bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
+	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
+	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
+
+	//setters
+	void setSimilarityThreshold(float similarityThreshold);
+	void setSimilarityOnlyLast(int similarityOnlyLast) {_similarityOnlyLast = similarityOnlyLast;}
+	void setOldSignatureRatio(float oldSignatureRatio);
+	void setMaxStMemSize(unsigned int maxStMemSize);
+	void setDelayRequired(int delayRequired);
+	void setRecentWmRatio(float recentWmRatio);
+	void setCommonSignatureUsed(bool commonSignatureUsed);
+	void setRawDataKept(bool rawDataKept) {_rawDataKept = rawDataKept;}
+
+	void dumpMemoryTree(const char * fileNameTree) const;
+	virtual void dumpMemory(std::string directory) const;
+	virtual void dumpSignatures(const char * fileNameSign) const {}
+	void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
+	void cleanLocalGraph(int id, unsigned int margin);
+	void cleanLTM(int maxDepth = 10);
+	void createGraph(Node * parent, unsigned int maxDepth, const std::set<int> & endIds = std::set<int>());
+
+protected:
+	virtual void preUpdate();
+	virtual void postUpdate() {}
+	virtual void merge(const Signature * from, Signature * to, MergingStrategy s) = 0;
+
+	virtual void addSignatureToStm(Signature * signature, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >());
+	virtual void clear();
+	virtual void moveToTrash(Signature * s);
+	virtual Signature * getSignatureLtMem(int id);
+
+	void addSignatureToWm(Signature * signature);
+	Signature * _getSignature(int id) const;
+	Signature * _getLastSignature();
+	Signature * getRemovableSignature(const std::list<int> & ignoredIds = std::list<int>(), bool onlyLoopedSignatures = false);
+	int getNextId();
+	void initCountId();
+	int rehearsal(const Signature * signature, bool onlyLast, float & similarity);
+	void touch(int signatureId);
+	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
+
+private:
+	void createVirtualSignature(Signature ** signature);
+	virtual Signature * createSignature(int id, const SMState * rawData, bool keepRawData=false) = 0;
+	void cleanGraph(const Node * root);
+protected:
+	DBDriver * _dbDriver;
+
+private:
+	float _similarityThreshold;
+	bool _similarityOnlyLast;
+	bool _rawDataKept;
+	int _idCount;
+	Signature * _lastSignature;
+	int _lastLoopClosureId;
+	bool _incrementalMemory;
+	unsigned int _maxStMemSize;
+	bool _commonSignatureUsed;
+	bool _databaseCleaned; //if true, delete old signatures in the database
+	int _delayRequired;
+	float _recentWmRatio;
+	bool _memoryChanged; // False by default, become true when Memory::update() is called.
+	bool _merging;
+	int _signaturesAdded;
+
+	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
+	std::set<int> _stMem;
+	std::map<int, int> _workingMem; // id, timeStamp
+};
+
+} // namespace rtabmap
+
+#endif /* MEMORY_H_ */

corelib/src/NearestNeighbor.cpp

@@ -0,0 +1,189 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "NearestNeighbor.h"
+#include "utilite/ULogger.h"
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+/////////////////////////
+// KdTreeNN
+/////////////////////////
+KdTreeNN::KdTreeNN(const ParametersMap & parameters) :
+		_tree(0)
+{
+	ULOGGER_DEBUG("");
+	this->parseParameters(parameters);
+}
+
+KdTreeNN::~KdTreeNN()
+{
+	if(_tree)
+	{
+		cvReleaseFeatureTree(_tree);
+	}
+}
+
+void KdTreeNN::setData(const cv::Mat & data)
+{
+	if(_tree)
+	{
+		cvReleaseFeatureTree(_tree);
+		_tree = 0;
+	}
+
+	// convert to old style mat (data is not copied)
+	_dataMat = data;
+	_tree = cvCreateKDTree(&_dataMat);
+}
+
+void KdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
+{
+	ULOGGER_DEBUG("");
+	if(_tree)
+	{
+		// convert to old style mat (data is not copied)
+		CvMat queriesMat = queries;
+		CvMat indicesMat = indices;
+		CvMat distsMat = dists;
+		cvFindFeatures(_tree, &queriesMat, &indicesMat, &distsMat, knn, emax);
+	}
+	else
+	{
+		ULOGGER_ERROR("The search tree is not created, setData() must be called first");
+	}
+}
+
+void KdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
+{
+	ULOGGER_DEBUG("");
+	CvMat dataMat = data;
+	CvFeatureTree * tree = cvCreateKDTree(&dataMat);
+
+	if(tree)
+	{
+		// convert to old style mat (data is not copied)
+		CvMat queriesMat = queries;
+		CvMat indicesMat = indices;
+		CvMat distsMat = dists;
+		cvFindFeatures(tree, &queriesMat, &indicesMat, &distsMat, knn, emax);
+		cvReleaseFeatureTree(tree);
+	}
+	else
+	{
+		ULOGGER_ERROR("The search tree creation failed ?!?");
+	}
+}
+
+void KdTreeNN::parseParameters(const ParametersMap & parameters)
+{
+	NearestNeighbor::parseParameters(parameters);
+}
+
+
+
+
+/////////////////////////
+// FlannKdTreeNN
+/////////////////////////
+FlannKdTreeNN::FlannKdTreeNN(const ParametersMap & parameters) :
+		_treeFlannIndex(0),
+		_strategy(kKDTree)
+{
+	ULOGGER_DEBUG("");
+	this->parseParameters(parameters);
+}
+
+FlannKdTreeNN::~FlannKdTreeNN() {
+	if(_treeFlannIndex)
+	{
+		delete _treeFlannIndex;
+	}
+}
+
+void FlannKdTreeNN::setData(const cv::Mat & data)
+{
+	if(_treeFlannIndex)
+	{
+		delete _treeFlannIndex;
+		_treeFlannIndex = 0;
+	}
+
+	_treeFlannIndex = createIndex(data, _strategy);  // using 4 randomized trees
+	//_treeFlannIndex = new cv::flann::Index(_dataTree, cv::flann::AutotunedIndexParams(0.9, 0.01, 0, 0.1));  // use autotuned parameters
+}
+
+void FlannKdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
+{
+	ULOGGER_DEBUG("");
+	if(_treeFlannIndex)
+	{
+		// Note, the search params is ignored because we use an autotuned created index (see update())
+		_treeFlannIndex->knnSearch(queries, indices, dists, knn, cv::flann::SearchParams(emax) ); // maximum number of leafs checked
+	}
+	else
+	{
+		ULOGGER_ERROR("The search index is not created, setData() must be called first");
+	}
+}
+
+void FlannKdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
+{
+	ULOGGER_DEBUG("");
+	cv::flann::Index * index = createIndex(data, _strategy);
+	// Note, the search params is ignored because we use an autotuned created index (see update())
+	index->knnSearch(queries, indices, dists, knn, cv::flann::SearchParams(emax) ); // maximum number of leafs checked
+	delete index;
+}
+
+void FlannKdTreeNN::parseParameters(const ParametersMap & parameters)
+{
+	NearestNeighbor::parseParameters(parameters);
+}
+
+enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
+cv::flann::Index * FlannKdTreeNN::createIndex(const cv::Mat & data, Strategy s) const
+{
+	cv::flann::Index * index = 0;
+	switch(s)
+	{
+	case kLinear:
+		index = new cv::flann::Index(data, cv::flann::LinearIndexParams());
+		break;
+	case kKDTree:
+		index = new cv::flann::Index(data, cv::flann::KDTreeIndexParams());
+		break;
+	case kMeans:
+		index = new cv::flann::Index(data, cv::flann::KMeansIndexParams());
+		break;
+	case kComposite:
+		index = new cv::flann::Index(data, cv::flann::CompositeIndexParams());
+		break;
+	case kAutoTuned:
+	default:
+		index = new cv::flann::Index(data, cv::flann::AutotunedIndexParams());
+		break;
+	}
+	return index;
+}
+
+
+}

corelib/src/NearestNeighbor.h

@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef NEARESTNEIGHBOR_H_
+#define NEARESTNEIGHBOR_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc_c.h>
+#include <map>
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap
+{
+
+class VisualWord;
+
+class RTABMAP_EXP NearestNeighbor
+{
+public:
+
+public:
+	virtual ~NearestNeighbor() {}
+
+	virtual void setData(const cv::Mat & data) = 0;
+
+	virtual void search(
+			const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64) = 0;
+
+	virtual void search(
+			const cv::Mat & data,
+			const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64) const = 0;
+
+	virtual bool isDist64F() const = 0;
+	virtual bool isDistSquared() const = 0;
+
+	virtual void parseParameters(const ParametersMap & parameters) {}
+
+protected:
+	NearestNeighbor() {}
+};
+
+
+
+/////////////////////////
+// KdTreeNN
+/////////////////////////
+class RTABMAP_EXP KdTreeNN : public NearestNeighbor
+{
+public:
+	KdTreeNN(const ParametersMap & parameters = ParametersMap());
+	virtual ~KdTreeNN();
+
+	virtual void setData(const cv::Mat & data);
+
+	virtual void search(const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64);
+
+	virtual void search(const cv::Mat & data,
+			const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64) const;
+
+	virtual bool isDist64F() const {return true;}
+	virtual bool isDistSquared() const {return false;}
+	virtual void parseParameters(const ParametersMap & parameters);
+private:
+	CvFeatureTree * _tree;
+	CvMat _dataMat;
+};
+
+
+
+
+
+
+/////////////////////////
+// FlannKdTreeNN
+/////////////////////////
+class RTABMAP_EXP FlannKdTreeNN : public NearestNeighbor
+{
+public:
+	enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
+
+public:
+	FlannKdTreeNN(const ParametersMap & parameters = ParametersMap());
+	FlannKdTreeNN(Strategy s, const ParametersMap & parameters = ParametersMap());
+	virtual ~FlannKdTreeNN();
+
+	void setStrategy(Strategy s) {if(_strategy!=kUndefined) _strategy = s;}
+
+	virtual void setData(const cv::Mat & data);
+
+	virtual void search(const cv::Mat & queries,
+			cv::Mat & indices,
+			cv::Mat & dists,
+			int knn = 1,
+			int emax = 64);
+
+	virtual void search(const cv::Mat & data,
+				const cv::Mat & queries,
+				cv::Mat & indices,
+				cv::Mat & dists,
+				int knn = 1,
+				int emax = 64) const;
+
+	virtual bool isDist64F() const {return false;}
+	virtual bool isDistSquared() const {return true;}
+	virtual void parseParameters(const ParametersMap & parameters);
+
+private:
+	cv::flann::Index * createIndex(const cv::Mat & data, Strategy s) const;
+
+private:
+	cv::flann::Index * _treeFlannIndex;
+	Strategy _strategy;
+};
+
+}
+
+#endif /* NEARESTNEIGHBOR_H_ */

corelib/src/Node.h

@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef NODE_H_
+#define NODE_H_
+
+namespace rtabmap {
+
+class Node
+{
+public:
+	Node(int id, Node * parent = 0) :
+		_parent(parent),
+		_id(id)
+	{
+		if(_parent)
+		{
+			_parent->addChild(this);
+		}
+	}
+	virtual ~Node()
+	{
+		//We copy the set because when a child is destroyed, it is removed from its parent.
+		std::set<Node*> children = _children;
+		_children.clear();
+		for(std::set<Node*>::iterator iter=children.begin(); iter!=children.end(); ++iter)
+		{
+			delete *iter;
+		}
+		children.clear();
+		if(_parent)
+		{
+			_parent->removeChild(this);
+		}
+	}
+	int id() const {return _id;}
+	bool isAncestor(int id) const
+	{
+		if(_parent)
+		{
+			if(_parent->id() == id)
+			{
+				return true;
+			}
+			return _parent->isAncestor(id);
+		}
+		return false;
+	}
+
+	void expand(std::list<std::list<int> > & paths, std::list<int> currentPath = std::list<int>()) const
+	{
+		currentPath.push_back(_id);
+		if(_children.size() == 0)
+		{
+			paths.push_back(currentPath);
+			return;
+		}
+		for(std::set<Node*>::const_iterator iter=_children.begin(); iter!=_children.end(); ++iter)
+		{
+			(*iter)->expand(paths, currentPath);
+		}
+	}
+
+private:
+	void addChild(Node * child)
+	{
+		_children.insert(child);
+	}
+	void removeChild(Node * child)
+	{
+		_children.erase(child);
+	}
+
+private:
+	std::set<Node*> _children;
+	Node * _parent;
+	int _id;
+};
+
+}
+
+#endif /* NODE_H_ */

corelib/src/Parameters.cpp

@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/Parameters.h"
+#include <utilite/UDirectory.h>
+#include <utilite/ULogger.h>
+
+namespace rtabmap
+{
+
+Parameters * Parameters::instance_ = 0;
+UDestroyer<Parameters> Parameters::destroyer_;
+ParametersMap Parameters::parameters_;
+
+Parameters::Parameters()
+{
+}
+
+Parameters::~Parameters()
+{
+}
+
+const ParametersMap & Parameters::getDefaultParameters()
+{
+	return Parameters::getInstance()->getParameters();
+}
+
+Parameters * Parameters::getInstance()
+{
+	if(!instance_)
+	{
+		instance_ = new Parameters();
+		destroyer_.setDoomed(instance_);
+	}
+	return instance_;
+}
+
+const ParametersMap & Parameters::getParameters() const
+{
+	return parameters_;
+}
+
+void Parameters::addParameter(const std::string & key, const std::string & value)
+{
+	parameters_.insert(ParametersPair(key, value));
+}
+
+std::string Parameters::getDefaultWorkingDirectory()
+{
+	std::string path = UDirectory::homeDir();
+	if(!path.empty())
+	{
+		UDirectory::makeDir(path += "/Documents");
+		UDirectory::makeDir(path += "/RTAB-Map");
+		path += "/"; // add trailing separator
+	}
+	else
+	{
+		UFATAL("Can't get the HOME variable environment!");
+	}
+	return path;
+}
+
+}

corelib/src/RtabmapEvent.cpp

@@ -0,0 +1,150 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "rtabmap/core/RtabmapEvent.h"
+
+namespace rtabmap {
+std::map<std::string, float> Statistics::_defaultData;
+bool Statistics::_defaultDataInitialized = false;
+
+const std::map<std::string, float> & Statistics::defaultData()
+{
+	Statistics stat;
+	return _defaultData;
+}
+
+Statistics::Statistics() :
+	_extended(0),
+	_refImageId(0),
+	_loopClosureId(0),
+	_refImage(0),
+	_loopClosureImage(0)
+{
+	_defaultDataInitialized = true;
+}
+Statistics::Statistics(const Statistics & s) :
+	_extended(0),
+	_refImageId(0),
+	_loopClosureId(0),
+	_refImage(0),
+	_loopClosureImage(0)
+{
+	*this = s;
+}
+Statistics::~Statistics()
+{
+	if(_refImage)
+	{
+		cvReleaseImage(&_refImage);
+	}
+	if(_loopClosureImage)
+	{
+		cvReleaseImage(&_loopClosureImage);
+	}
+}
+
+// name format = "Grp/Name/unit"
+void Statistics::addStatistic(const std::string & name, float value)
+{
+	_data.insert(std::pair<std::string, float>(name, value));
+}
+
+//take the ownership of the image, the image will be
+//deleted in the 'Statistics' destructor
+void Statistics::setRefImage(IplImage ** refImage)
+{
+	if(_refImage)
+		cvReleaseImage(&_refImage);
+	_refImage = *refImage;
+}
+
+// Copy the image
+void Statistics::setRefImage(const IplImage * refImage)
+{
+	if(_refImage)
+		cvReleaseImage(&_refImage);
+	if(refImage)
+	{
+		_refImage = cvCloneImage(refImage);
+	}
+	else
+	{
+		_refImage = 0;
+	}
+}
+
+//take the ownership of the image, the image will be
+//deleted in the 'Statistics' destructor
+void Statistics::setLoopClosureImage(IplImage ** loopClosureImage)
+{
+	if(_loopClosureImage)
+		cvReleaseImage(&_loopClosureImage);
+	_loopClosureImage = *loopClosureImage;
+}
+
+// Copy the image
+void Statistics::setLoopClosureImage(const IplImage * loopClosureImage)
+{
+	if(_loopClosureImage)
+		cvReleaseImage(&_loopClosureImage);
+	if(loopClosureImage)
+	{
+		_loopClosureImage = cvCloneImage(loopClosureImage);
+	}
+	else
+	{
+		_loopClosureImage = 0;
+	}
+}
+
+Statistics & Statistics::operator=(const Statistics & s)
+{
+	ULOGGER_DEBUG("");
+	_data = s.data();
+	if(_refImage)
+	{
+		cvReleaseImage(&_refImage);
+		_refImage = 0;
+	}
+	if(_loopClosureImage)
+	{
+		cvReleaseImage(&_loopClosureImage);
+		_loopClosureImage = 0;
+	}
+	_extended = s.extended();
+	_refImageId = s.refImageId();
+	_loopClosureId = s.loopClosureId();
+	if(s.refImage())
+	{
+		_refImage = cvCloneImage(s.refImage());
+	}
+	if(s.loopClosureImage())
+	{
+		_loopClosureImage = cvCloneImage(s.loopClosureImage());
+	}
+	_posterior = s.posterior();
+	_likelihood = s.likelihood();
+	_weights = s.weights();
+	_refWords = s.refWords();
+	_loopWords = s.loopWords();
+
+	return *this;
+}
+
+}

corelib/src/Signature.cpp

@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "Signature.h"
+#include "Memory.h"
+#include <opencv2/highgui/highgui.hpp>
+#include "VerifyHypotheses.h"
+
+#include "utilite/UtiLite.h"
+
+namespace rtabmap
+{
+
+Signature::~Signature()
+{
+	ULOGGER_DEBUG("id=%d", _id);
+	if(_image)
+	{
+		cvReleaseImage(&_image);
+	}
+}
+
+Signature::Signature(int id, const IplImage * image, bool keepImage) :
+		_id(id),
+		_weight(0),
+		_loopClosureId(0),
+		_image(0),
+		_saved(false),
+		_width(0),
+		_height(0)
+{
+	if(image)
+	{
+		_width = image->width;
+		_height = image->height;
+		if(keepImage)
+		{
+			_image = cvCloneImage(image);
+		}
+	}
+}
+
+// Warning, the image returned must be released
+const IplImage * Signature::getImage() const
+{
+	return _image;
+}
+
+void Signature::setImage(const IplImage * image)
+{
+	if(_image && image)
+	{
+		cvReleaseImage(&_image);
+		_image = cvCloneImage(image);
+	}
+	else
+	{
+		UWARN("Parameter is null or no image is saved.");
+	}
+}
+
+// Warning, the matrix returned must be released
+CvMat * Signature::compressImage(const IplImage * image)
+{
+	if(!image)
+	{
+		UERROR("The parameter must not be null.");
+		return 0;
+	}
+	// Compress image
+	int params[3] = {0};
+
+	//JPEG compression
+	std::string format = "jpeg";
+	params[0] = CV_IMWRITE_JPEG_QUALITY;
+	params[1] = 80; // default: 80% quality
+
+	//PNG compression
+	//std::string format = "png";
+	//params[0] = CV_IMWRITE_PNG_COMPRESSION;
+	//params[1] = 9; // default: maximum compression
+
+	std::string extension = '.' + format;
+	return cvEncodeImage(extension.c_str(), image, params);
+}
+
+// Warning, the image returned must be released
+IplImage * Signature::decompressImage(const CvMat * imageCompressed)
+{
+	if(!imageCompressed)
+	{
+		UERROR("The parameter must not be null.");
+		return 0;
+	}
+	return cvDecodeImage(imageCompressed, CV_LOAD_IMAGE_ANYCOLOR);
+}
+
+void Signature::addNeighbors(const NeighborsMap & neighbors)
+{
+	for(NeighborsMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+	{
+		this->addNeighbor(i->first, i->second);
+		//UDEBUG("%d -> %d, a=%d", this->id(), i->first, i->second.size());
+	}
+}
+
+void Signature::addNeighbor(int neighbor, const std::list<std::vector<float> > & actions)
+{
+	ULOGGER_DEBUG("Adding neighbor %d to %d with %d actions", neighbor, this->id(), actions.size());
+	std::pair<NeighborsMap::iterator, bool> inserted = _neighbors.insert(std::pair<int, std::list<std::vector<float> > >(neighbor, actions));
+	//UDEBUG("%d -> %d, a=%d", this->id(), neighbor, actions.size());
+	if(!inserted.second)
+	{
+		ULOGGER_ERROR("neighbor %d already added to %d", neighbor, this->id());
+		return;
+	}
+	if(neighbor == _id)
+	{
+		ULOGGER_ERROR("same Id ? (%d)", neighbor, this->id());
+		return;
+	}
+}
+
+void Signature::removeNeighbor(int neighbor)
+{
+	ULOGGER_DEBUG("Removing neighbor %d to %d", neighbor, this->id());
+	// we delete the first found because there is not supposed
+	// to have more than one occurrence of this neighbor (see addNeighbor())
+	int erased = _neighbors.erase(neighbor);
+	if(!erased)
+	{
+		ULOGGER_WARN("neighbor %d not found in %d", neighbor, this->id());
+	}
+}
+
+
+
+
+//KeypointSignature
+KeypointSignature::KeypointSignature(
+		const std::multimap<int, cv::KeyPoint> & words,
+		int id,
+		const IplImage * image,
+		bool keepRawData) :
+	Signature(id, image, keepRawData),
+	_words(words),
+	_enabled(false)
+{
+}
+
+KeypointSignature::KeypointSignature(int id) :
+	Signature(id),
+	_enabled(false)
+{
+}
+
+KeypointSignature::~KeypointSignature()
+{
+}
+
+float KeypointSignature::compareTo(const Signature * s) const
+{
+	const KeypointSignature * ss = dynamic_cast<const KeypointSignature *>(s);
+	float similarity = 0;
+
+	if(ss) //Compatible
+	{
+		const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
+
+		if(words.size() != 0 && _words.size() != 0)
+		{
+			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);
+
+			similarity = float(pairs.size()) / float(totalWords);
+
+			// Adjust similarity with the ratio of words between the signatures
+			/*float ratio = 1;
+			if(_words.size() > words.size() && _words.size())
+			{
+				ratio = float(words.size()) / float(_words.size());
+			}
+			else
+			{
+				ratio = float(_words.size()) / float(words.size());
+			}
+
+			similarity *= ratio;*/
+		}
+	}
+	return similarity;
+}
+
+void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
+{
+	std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
+	if(kps.size())
+	{
+		_words.erase(oldWordId);
+		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
+		{
+			_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
+		}
+	}
+}
+
+#define BAD_SIGNATURE_THRESHOLD 0 // elements
+bool KeypointSignature::isBadSignature() const
+{
+	if(_words.size() <= BAD_SIGNATURE_THRESHOLD)
+		return true;
+	return false;
+}
+
+void KeypointSignature::removeAllWords()
+{
+	_words.clear();
+}
+
+void KeypointSignature::removeWord(int wordId)
+{
+	_words.erase(wordId);
+}
+
+}

corelib/src/Signature.h

@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <map>
+#include <list>
+#include <vector>
+
+//TODO : add copy constructor
+
+namespace rtabmap
+{
+
+class Memory;
+typedef std::map<int, std::list<std::vector<float> > > NeighborsMap;
+
+class RTABMAP_EXP Signature
+{
+public:
+	static CvMat * compressImage(const IplImage * image);
+	static IplImage * decompressImage(const CvMat * imageCompressed);
+
+public:
+	virtual ~Signature();
+
+	/**
+	 * Must return a value between >=0 and <=1 (1 means 100% similarity)
+	 */
+	virtual float compareTo(const Signature * signature) const = 0;
+	virtual bool isBadSignature() const = 0;
+	virtual std::string signatureType() const = 0;
+
+	const IplImage * getImage() const;
+	void setImage(const IplImage * image);
+
+	int id() const {return _id;}
+
+	void addNeighbors(const NeighborsMap & neighbors);
+	void addNeighbor(int neighborId, const std::list<std::vector<float> > & actions);
+	void removeNeighbor(int neighborId);
+	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
+	void setWeight(int weight) {_weight = weight;}
+	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
+	void setWidth(int width) {_width = width;}
+	void setHeight(int height) {_height = height;}
+	void setSaved(bool saved) {_saved = saved;}
+
+	const NeighborsMap & getNeighbors() const {return _neighbors;}
+	int getWeight() const {return _weight;}
+	int getLoopClosureId() const {return _loopClosureId;}
+	int getWidth() const {return _width;}
+	int getHeight() const {return _height;}
+	bool isSaved() const {return _saved;}
+
+protected:
+	Signature(int id, const IplImage * image = 0, bool keepImage = false);
+
+private:
+	int _id;
+	NeighborsMap _neighbors; // id, [action1, action2, ...] All actions must have the same length
+	int _weight;
+	int _loopClosureId;
+	IplImage * _image;
+	bool _saved; // If it's saved to bd
+	int _width; // pixels
+	int _height; // pixels
+};
+
+
+class KeypointDetector;
+class VWDictionary;
+
+class RTABMAP_EXP KeypointSignature :
+	public Signature
+{
+public:
+	KeypointSignature(
+			const std::multimap<int, cv::KeyPoint> & words,
+			int id,
+			const IplImage * image = 0,
+			bool keepRawData = false);
+	KeypointSignature(int id);
+
+	virtual ~KeypointSignature();
+
+	virtual float compareTo(const Signature * signature) const;
+	virtual bool isBadSignature() const;
+	virtual std::string signatureType() const {return "KeypointSignature";};
+
+	void removeAllWords();
+	void removeWord(int wordId);
+	void changeWordsRef(int oldWordId, int activeWordId);
+
+	void setWords(const std::multimap<int, cv::KeyPoint> & words) {_enabled = false;_words = words;}
+	bool isEnabled() const {return _enabled;}
+	void setEnabled(bool enabled) {_enabled = enabled;}
+	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
+
+private:
+	// Contains all words (Some can be duplicates -> if a word appears 2
+	// times in the signature, it will be 2 times in this list)
+	// Words match with the CvSeq keypoints and descriptors
+	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
+	bool _enabled;
+
+};
+
+
+} // namespace rtabmap

corelib/src/VWDictionary.h

@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "VisualWord.h"
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap
+{
+
+class NearestNeighbor;
+class DBDriver;
+
+class RTABMAP_EXP VWDictionary
+{
+public:
+	enum NNStrategy{kNNNaive, kNNKdTree, kNNFlannKdTree, kNNUndef};
+	static const int ID_START;
+	static const int ID_INVALID;
+
+public:
+	VWDictionary(const ParametersMap & parameters = ParametersMap());
+	virtual ~VWDictionary();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	virtual void update();
+
+	virtual std::list<int> addNewWords(
+			const std::list<std::vector<float> > & descriptors,
+			unsigned int dim,
+			int signatureId);
+	virtual void addWord(VisualWord * vw);
+
+	virtual std::vector<int> findNN(const std::list<VisualWord *> & vws, bool searchInNewlyAddedWords = true) const;
+	void naiveNNSearch(const std::list<VisualWord *> & words, const float * d, unsigned int length, std::map<float, int> & results, unsigned int k) const;
+
+	void addWordRef(int wordId, int signatureId);
+	void removeAllWordRef(int wordId, int signatureId);
+	const VisualWord * getWord(int id) const;
+	void setWordSaved(int id, bool saved);
+	void setLastWordId(int id) {_lastWordId = id;}
+	void getCommonWords(unsigned int nbCommonWords, int totalSign, std::list<int> & commonWords) const;
+	const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
+	void setMinDist(float d);
+	float getMinDist() const {return _minDist;}
+	bool isMinDistUsed() const {return _minDistUsed;}
+	void setMinDistUsed(bool used) {_minDistUsed = used;}
+	void setNndrUsed(bool used) {_nndrUsed = used;}
+	bool isNndrUsed() const {return _nndrUsed;}
+	void setNndrRatio(float ratio);
+	float getNndrRatio() {return _nndrRatio;}
+	unsigned int getNotIndexedWordsCount() const {return _visualWords.size() - _mapIndexId.size();}
+	unsigned int getLastNewWordsAddedCount() const {return _lastNewWordsAddedCount;}
+	int getLastIndexedWordId() const;
+	int getTotalActiveReferences() const {return _totalActiveReferences;}
+	void setNNStrategy(NNStrategy strategy, const ParametersMap & parameters = ParametersMap());
+	NNStrategy nnStrategy() const;
+	bool isIncremental() const {return _incrementalDictionary;}
+	void setIncrementalDictionary(bool incrementalDictionary, const std::string & dictionaryPath);
+
+	void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
+
+	void clear();
+	std::vector<VisualWord *> getUnusedWords() const;
+	unsigned int getUnusedWordsSize() const {return _unusedWords.size();}
+	void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
+
+protected:
+	int getNextId();
+
+protected:
+	std::map<int, VisualWord *> _visualWords; //<id,VisualWord*>
+	unsigned int _lastNewWordsAddedCount;
+	int _totalActiveReferences; // keep track of all references for updating the common signature
+
+private:
+	bool _incrementalDictionary;
+	bool _minDistUsed;
+	float _minDist; //euclidean distance ^ 2
+	bool _nndrUsed;
+	float _nndrRatio;
+	unsigned int _maxLeafs;
+	std::string _dictionaryPath; // a pre-computed dictionary (.txt)
+	unsigned int _dim;
+	int _lastWordId;
+	NearestNeighbor * _nn;
+	cv::Mat _dataTree;
+	std::map<int ,int> _mapIndexId;
+	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>
+};
+
+} // namespace rtabmap

corelib/src/VerifyHypotheses.cpp

@@ -0,0 +1,595 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "VerifyHypotheses.h"
+#include "rtabmap/core/Parameters.h"
+#include "Signature.h"
+#include "Memory.h"
+#include <cstdlib>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include "utilite/UtiLite.h"
+
+namespace rtabmap
+{
+
+VerifyHypotheses::VerifyHypotheses(const ParametersMap & parameters) :
+	_status(0)
+{
+	this->parseParameters(parameters);
+}
+
+void VerifyHypotheses::parseParameters(const ParametersMap & parameters)
+{
+}
+
+
+
+/////////////////////////
+// VerifyHypothesesSimple
+/////////////////////////
+VerifyHypothesesSimple::VerifyHypothesesSimple(const ParametersMap & parameters) :
+	VerifyHypotheses(parameters)
+{
+	this->parseParameters(parameters);
+}
+
+VerifyHypothesesSimple::~VerifyHypothesesSimple()
+{
+}
+
+void VerifyHypothesesSimple::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())
+	{
+		_seqLength = atoi((*iter).second.c_str());
+	}
+	VerifyHypotheses::parseParameters(parameters);
+}
+
+int VerifyHypothesesSignSeq::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
+{
+	int hypothesis = 0;
+	std::map<int, int> hypothesesToKeep;
+
+	// 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
+		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;
+
+	// 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
+/////////////////////////
+VerifyHypothesesEpipolarGeo::VerifyHypothesesEpipolarGeo(const ParametersMap & parameters) :
+	VerifyHypotheses(parameters),
+	_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
+	_ransacParam1(Parameters::defaultVhEpRansacParam1()),
+	_ransacParam2(Parameters::defaultVhEpRansacParam2())
+{
+	this->parseParameters(parameters);
+}
+
+VerifyHypothesesEpipolarGeo::~VerifyHypothesesEpipolarGeo() {
+
+}
+
+void VerifyHypothesesEpipolarGeo::parseParameters(const ParametersMap & parameters)
+{
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
+	{
+		_matchCountMinAccepted = std::atoi((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kVhEpRansacParam1())) != parameters.end())
+	{
+		_ransacParam1 = std::atof((*iter).second.c_str());
+	}
+	if((iter=parameters.find(Parameters::kVhEpRansacParam2())) != parameters.end())
+	{
+		_ransacParam2 = std::atof((*iter).second.c_str());
+	}
+	VerifyHypotheses::parseParameters(parameters);
+}
+
+void VerifyHypothesesEpipolarGeo::setStatus(int status)
+{
+	// Only set if the status was not set before. This will keep the
+	// first error (when comparing with many signatures)
+	if(status == UNDEFINED || this->getStatus() == UNDEFINED || status == ACCEPTED)
+	{
+		VerifyHypotheses::setStatus(status);
+	}
+}
+
+int VerifyHypothesesEpipolarGeo::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
+{
+	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());
+
+	std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
+	std::list<int> pairsId;
+
+	//bool allPairs = true;
+	int realPairsCount = 0;
+
+
+	realPairsCount = findPairsOne(ssA->getWords(), ssB->getWords(), pairs, pairsId);
+	ULOGGER_DEBUG("%d %d", pairs.size(), pairsId.size());
+	int pairsCount = pairs.size();
+	ULOGGER_DEBUG("id(%d,%d) realPairsCount found=%d, pairsCount=%d...", ssA->id(), ssB->id(), realPairsCount, pairsCount);
+
+	int similarities = this->getTotalSimilarities(ssA->getWords(), ssB->getWords());
+
+	ULOGGER_DEBUG("realPairsCount=%d, "
+			"test1=%f%%, "
+			"test2=%f%%, "
+			"similarities/total=%f%%, "
+			"realP/similarities=%f%%, "
+			"(pairs/2)/similarities=%f%%",
+			realPairsCount,
+			float(realPairsCount)/(float(ssA->getWords().size() + ssB->getWords().size())/2),
+			float(pairs.size())/(float(ssA->getWords().size() + ssB->getWords().size())/2),
+			float(similarities)/float(ssA->getWords().size() + ssB->getWords().size()),
+			float(realPairsCount) / float(similarities),
+			float(pairs.size()) / float(similarities));
+	if(pairsCount < _matchCountMinAccepted)
+	{
+		this->setStatus(this->NOT_ENOUGH_MATCHING_PAIRS);
+		return false;
+	}
+
+	//Convert Keypoints to a structure that OpenCV understands
+	//3 dimensions (Homogeneous vectors)
+	cv::Mat points1(1, pairs.size(), CV_32FC2);
+	cv::Mat points2(1, pairs.size(), CV_32FC2);
+
+	float * points1data = points1.ptr<float>(0);
+	float * points2data = points2.ptr<float>(0);
+
+	// Fill the points here ...
+	int i=0;
+	for(std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::const_iterator iter = pairs.begin();
+		iter != pairs.end();
+		++iter )
+	{
+		points1data[i*2] = (*iter).first.pt.x;
+		points1data[i*2+1] = (*iter).first.pt.y;
+
+		points2data[i*2] = (*iter).second.pt.x;
+		points2data[i*2+1] = (*iter).second.pt.y;
+
+		// the output of the correspondences can be easily copied in MatLab
+		/*if(i==0)
+		{
+			ULOGGER_DEBUG("pt x=[%f;%f;1;%d];,xp=[%f;%f;1;%d];",
+					(*iter).first.pt.x,
+					(*iter).first.pt.y,
+					Util::valueAt(pairsId,i),
+					(*iter).second.pt.x,
+					(*iter).second.pt.y,
+					Util::valueAt(pairsId,i));
+		}
+		else
+		{
+			ULOGGER_DEBUG("pt x=[x [%f;%f;1;%d]];,xp=[xp [%f;%f;1;%d]];",
+					(*iter).first.pt.x,
+					(*iter).first.pt.y,
+					Util::valueAt(pairsId,i),
+					(*iter).second.pt.x,
+					(*iter).second.pt.y,
+					Util::valueAt(pairsId,i));
+		}*/
+		++i;
+	}
+
+	UTimer timer;
+	timer.start();
+
+	// Find the fundamental matrix
+	cv::vector<uchar> status;
+	cv::Mat fundamentalMatrix = cv::findFundamentalMat(
+				points1,
+				points2,
+				status,
+				CV_FM_RANSAC,
+				_ransacParam1,
+				_ransacParam2);
+
+	ULOGGER_DEBUG("Find fundamental matrix (OpenCV) time = %fs", timer.ticks());
+
+		// Fundamental matrix is valid ?
+	bool fundMatFound = false;
+	if(fundamentalMatrix.type() != CV_64FC1)
+	{
+		ULOGGER_FATAL("fundamentalMatrix.type() != CV_64FC1");
+	}
+	if(fundamentalMatrix.cols==3 && fundamentalMatrix.rows==3 &&
+	   (fundamentalMatrix.at<double>(0,0) != 0.0 ||
+	    fundamentalMatrix.at<double>(0,1) != 0.0 ||
+	    fundamentalMatrix.at<double>(0,2) != 0.0 ||
+	    fundamentalMatrix.at<double>(1,0) != 0.0 ||
+	    fundamentalMatrix.at<double>(1,1) != 0.0 ||
+	    fundamentalMatrix.at<double>(1,2) != 0.0 ||
+		fundamentalMatrix.at<double>(2,0) != 0.0 ||
+		fundamentalMatrix.at<double>(2,1) != 0.0 ||
+		fundamentalMatrix.at<double>(2,2) != 0.0) )
+
+	{
+		fundMatFound = true;
+	}
+
+	ULOGGER_DEBUG("id(%d,%d) fm_count=%d...", ssA->id(), ssB->id(), fundMatFound);
+
+	if(fundMatFound)
+	{
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > inliers;
+		std::list<int> inliersId;
+
+		int goodCount = 0;
+		float total = 0;
+		std::list<std::pair<float, float> > ptsAddedA;
+		std::list<std::pair<float, float> > ptsAddedB;
+		cv::Mat x(3, 1, fundamentalMatrix.type());
+		cv::Mat xp(1, 3, fundamentalMatrix.type());
+		int i=0;
+		for(std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
+		{
+			//if(status[i])
+			{
+				if(uContains(ptsAddedA, std::pair<float, float>((*iter).first.pt.x, (*iter).first.pt.y)))
+				{
+					ULOGGER_DEBUG("already added point [%f,%f,1]", (*iter).first.pt.x, (*iter).first.pt.y);
+				}
+				else if(uContains(ptsAddedB, std::pair<float, float>((*iter).second.pt.x, (*iter).second.pt.y)))
+				{
+					ULOGGER_DEBUG("already added point [%f,%f,1]", (*iter).second.pt.x, (*iter).second.pt.y);
+				}
+				else
+				{
+					double * xData = x.ptr<double>(0);
+					double * xpData = xp.ptr<double>(0);
+					xData[0] = (*iter).first.pt.x;
+					xData[1] = (*iter).first.pt.y;
+					xData[2] = 1;
+					xpData[0] = (*iter).second.pt.x;
+					xpData[1] = (*iter).second.pt.y;
+					xpData[2] = 1;
+					cv::Mat r = xp * (fundamentalMatrix * x);
+					//if((r->data.fl[0] < 0 ? -r->data.fl[0]:r->data.fl[0]) < 1000000)
+					{
+						// Add only once a pair for the same id, used when a point matches with more than one...
+						ptsAddedA.push_back(std::pair<float, float>((*iter).first.pt.x, (*iter).first.pt.y));
+						ptsAddedB.push_back(std::pair<float, float>((*iter).second.pt.x, (*iter).second.pt.y));
+						if(status[i])
+						{
+							inliers.push_back(*iter);
+							inliersId.push_back(uValueAt(pairsId, i));
+							goodCount++;
+						}
+						//ULOGGER_DEBUG("[%d] status=%d, r->data.fl[0]=%f, Added!", Util::valueAt(pairsId,i), status[i], r.ptr<double>(0)[0]);
+					}
+					/*else
+					{
+						ULOGGER_DEBUG("status=%d, r->data.fl[0]=%f, Not added!", status->data.ptr[i], r->data.fl[0]);
+					}*/
+					total+=(r.ptr<double>(0)[0] < 0 ? -r.ptr<double>(0)[0]:r.ptr<double>(0)[0]);
+				}
+			}
+			/*else
+			{
+				ULOGGER_DEBUG("VHEpipolarGeo::doEpipolarGeometry() status=%d", status[i]);
+			}*/
+			++i;
+		}
+
+		ULOGGER_DEBUG("pairs/realPairs=%d/%d -> %d%%, goodCount=%d -> %d%%, good/real = %d%%, totalMean=%f",
+				pairsCount,
+				realPairsCount,
+				int(float(pairsCount)/float(realPairsCount*100)),
+				goodCount,
+				int(float(goodCount)/float(pairsCount*100)),
+				int(float(goodCount)/float(realPairsCount*100)),
+				total/float(realPairsCount));
+
+		// Show the fundamental matrix
+		ULOGGER_DEBUG(
+			"F = [%f %f %f;%f %f %f;%f %f %f]",
+			fundamentalMatrix.ptr<double>(0)[0],
+			fundamentalMatrix.ptr<double>(0)[1],
+			fundamentalMatrix.ptr<double>(0)[2],
+			fundamentalMatrix.ptr<double>(0)[3],
+			fundamentalMatrix.ptr<double>(0)[4],
+			fundamentalMatrix.ptr<double>(0)[5],
+			fundamentalMatrix.ptr<double>(0)[6],
+			fundamentalMatrix.ptr<double>(0)[7],
+			fundamentalMatrix.ptr<double>(0)[8]);
+
+		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;
+}
+
+/**
+ * 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,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
+		std::list<int> & pairsId)
+{
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
+	std::multimap<int, cv::KeyPoint>::const_iterator iterB;
+	pairs.clear();
+	int realPairsCount = 0;
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		iterA = wordsA.find(*i);
+		iterB = wordsB.find(*i);
+		while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+		{
+			pairsId.push_back(*i);
+			pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second));
+			++iterA;
+			++iterB;
+			++realPairsCount;
+		}
+	}
+	return realPairsCount;
+}
+
+/**
+ * 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,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
+		std::list<int> & pairsId)
+{
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	int realPairsCount = 0;
+	pairs.clear();
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
+		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
+		if(ptsA.size() == 1 && ptsB.size() == 1)
+		{
+			pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front()));
+			pairsId.push_back(*i);
+			++realPairsCount;
+		}
+		else if(ptsA.size()>1 && ptsB.size()>1)
+		{
+			// just update the count
+			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+		}
+	}
+	return realPairsCount;
+}
+
+/**
+ * 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,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
+		std::list<int> & pairsId)
+{
+	UTimer timer;
+	timer.start();
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	pairs.clear();
+	int realPairsCount = 0;;
+	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+	{
+		std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
+		std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
+
+		realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+		for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+		{
+			for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+			{
+				pairsId.push_back(*iter);
+				pairs.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter));
+			}
+		}
+	}
+	ULOGGER_DEBUG("time = %f", timer.ticks());
+	return realPairsCount;
+}
+
+/**
+ * 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,
+		const std::multimap<int, cv::KeyPoint> & wordsB)
+{
+	std::list<int> sameIds;
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		if(wordsB.find(*i) != wordsB.end())
+		{
+			sameIds.push_back(*i);
+		}
+	}
+	return sameIds;
+}
+
+int VerifyHypothesesEpipolarGeo::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;
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		total += uValues(wordsA, *i).size();
+		total += uValues(wordsB, *i).size();
+	}
+	return total;
+}
+
+}

corelib/src/VerifyHypotheses.h

@@ -0,0 +1,140 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef VERIFYHYPOTHESES_H_
+#define VERIFYHYPOTHESES_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <list>
+#include "rtabmap/core/Parameters.h"
+#include "utilite/UEventsHandler.h"
+#include <map>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+
+namespace rtabmap
+{
+
+class Memory;
+
+class RTABMAP_EXP VerifyHypotheses
+{
+public:
+	virtual ~VerifyHypotheses() {}
+	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem) = 0;
+
+	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
+/////////////////////////
+/*class VerifyHypothesesSignSeq : public VerifyHypotheses {
+public:
+	VerifyHypothesesSignSeq(const ParametersMap & parameters = ParametersMap());
+	virtual ~VerifyHypothesesSignSeq();
+	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
+	virtual void parseParameters(const ParametersMap & parameters);
+
+private:
+	std::map<int, int> _hypotheses;
+	int _seqLength;
+
+};*/
+
+
+
+/////////////////////////
+// VerifyHypothesesEpipolarGeo
+/////////////////////////
+class KeypointSignature;
+
+class RTABMAP_EXP VerifyHypothesesEpipolarGeo : public VerifyHypotheses
+{
+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());
+	virtual ~VerifyHypothesesEpipolarGeo();
+	virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	int getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
+
+	int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
+	double getRansacParam1() const {return _ransacParam1;}
+	double getRansacParam2() const {return _ransacParam2;}
+
+	void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
+	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);
+
+private:
+	int _matchCountMinAccepted;
+	double _ransacParam1;
+	double _ransacParam2;
+};
+
+} // namespace rtabmap
+
+#endif /* VERIFYHYPOTHESES_H_ */

corelib/src/VisualWord.cpp

@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "VisualWord.h"
+#include "utilite/ULogger.h"
+#include "utilite/UStl.h"
+
+namespace rtabmap
+{
+
+VisualWord::VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId) :
+	_id(id),
+	_saved(false),
+	_totalReferences(0)
+{
+	_descriptor = new float[dim];
+	if(_descriptor && descriptor)
+	{
+		memcpy(_descriptor, descriptor, dim*sizeof(float));
+	}
+	else
+	{
+		ULOGGER_ERROR("not enough memory to create the descriptor...");
+	}
+	_dim = dim;
+
+	if(signatureId)
+	{
+		addRef(signatureId);
+	}
+}
+
+VisualWord::~VisualWord()
+{
+	if(_descriptor)
+	{
+		delete [] _descriptor;
+	}
+}
+
+void VisualWord::addRef(int signatureId)
+{
+	std::map<int, int>::iterator iter = _references.find(signatureId);
+	if(iter != _references.end())
+	{
+		(*iter).second += 1;
+	}
+	else
+	{
+		_references.insert(std::pair<int, int>(signatureId, 1));
+	}
+	++_totalReferences;
+}
+
+int VisualWord::removeAllRef(int signatureId)
+{
+	int removed = uTake(_references, signatureId, 0);
+	_totalReferences -= removed;
+	return removed;
+}
+
+} // namespace rtabmap

corelib/src/VisualWord.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
+ *
+ * This file is part of RTAB-Map.
+ *
+ * RTAB-Map is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * RTAB-Map is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class SignatureSurf;
+
+class RTABMAP_EXP VisualWord
+{
+public:
+	VisualWord(int id, const float * descriptor, unsigned int dim, int signatureId = 0);
+	~VisualWord();
+
+	void addRef(int signatureId);
+	int removeAllRef(int signatureId);
+
+	int getTotalReferences() const {return _totalReferences;}
+	int id() const {return _id;}
+	const float * getDescriptor() const {return _descriptor;}
+	unsigned int getDim() const {return _dim;}
+	const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
+
+	bool isSaved() const {return _saved;}
+	void setSaved(bool saved) {_saved = saved;}
+
+private:
+	int _id;
+	float * _descriptor;
+	unsigned int _dim;
+	bool _saved; // If it's saved to bd
+
+	int _totalReferences;
+	std::map<int, int> _references; // (signature id , occurrence in the signature)
+	std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
+};
+
+} // namespace rtabmap

corelib/src/resources/DatabaseSchema.sql

@@ -0,0 +1,198 @@
+-- *******************************************************************
+--  construct_avpd_db: Script for creating the database
+--   Usage:
+--       $ sqlite3 AvpdDatabase.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Signature;
+DROP TABLE SignatureType;
+DROP TABLE Neighbor;
+DROP TABLE VisualWord;
+DROP TABLE Map_SS_VW;
+DROP TABLE StatisticsAfterRun;
+DROP TABLE StatisticsAfterRunSurf;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Signature (
+	id INTEGER NOT NULL,
+	type VARCHAR NOT NULL,
+	weight INTEGER,
+	loopClosureId INTEGER,
+	image BLOB,
+	imgWidth INTEGER,
+	imgHeight INTEGER,
+	timeEnter DATE,
+	PRIMARY KEY (id),
+	FOREIGN KEY (type) REFERENCES SignatureType(type),
+	FOREIGN KEY (loopClosureId) REFERENCES Signature(id)
+);
+
+CREATE TABLE Neighbor (
+    sid INTEGER NOT NULL,
+    nid INTEGER NOT NULL,
+    actionSize INTEGER,
+    actions BLOB,
+    timeEnter DATE,
+    PRIMARY KEY (sid, nid),
+    FOREIGN KEY (sid) REFERENCES Signature(id),
+    FOREIGN KEY (nid) REFERENCES Signature(id)
+);
+
+CREATE TABLE SignatureType (
+	type VARCHAR NOT NULL,
+	PRIMARY KEY (type)
+);
+
+CREATE TABLE VisualWord (
+	id INTEGER NOT NULL,
+	descriptorSize INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	timeEnter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Map_SS_VW (
+	signatureId INTEGER NOT NULL,
+	visualWordId INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	laplacian INTEGER NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	hessian FLOAT NOT NULL,
+	timeEnter DATE,
+	FOREIGN KEY (signatureId) REFERENCES Signature(id),
+	FOREIGN KEY (visualWordId) REFERENCES VisualWord(id)
+);
+
+CREATE TABLE StatisticsAfterRun (
+	stMemSize INTEGER,
+	lastSignAdded INTEGER,
+	processMemUsed INTEGER,
+	databaseMemUsed INTEGER,
+	timeEnter DATE
+);
+
+CREATE TABLE StatisticsAfterRunSurf (
+	dictionarySize INTEGER,
+	timeEnter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Signature BEFORE INSERT ON Signature 
+WHEN NOT EXISTS (SELECT type FROM SignatureType WHERE SignatureType.type = NEW.type)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed');
+END;
+
+CREATE TRIGGER insert_Neighbor BEFORE INSERT ON Neighbor 
+WHEN NOT EXISTS (SELECT id FROM Signature WHERE Signature.id = NEW.sid)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed');
+END;
+
+CREATE TRIGGER insert_Map_SS_VW BEFORE INSERT ON Map_SS_VW 
+WHEN NOT EXISTS (SELECT type FROM Signature WHERE Signature.id = NEW.signatureId AND type='surf')
+--OR NOT EXISTS (SELECT id FROM VisualWord WHERE VisualWord.id = NEW.visualWordId)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed');
+END;
+
+ --   Creating a trigger for timeEnter
+CREATE TRIGGER insert_Signature_timeEnter AFTER INSERT ON Signature
+BEGIN
+ UPDATE Signature SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Neighbor_timeEnter AFTER INSERT ON Neighbor
+BEGIN
+ UPDATE Neighbor SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_VisualWord_timeEnter AFTER INSERT ON VisualWord
+BEGIN
+ UPDATE VisualWord SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Map_SS_VW_timeEnter AFTER INSERT ON Map_SS_VW
+BEGIN
+ UPDATE Map_SS_VW SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_StatisticsAfterRun_timeEnter AFTER INSERT ON StatisticsAfterRun
+BEGIN
+ UPDATE StatisticsAfterRun SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_StatisticsAfterRunSurf_timeEnter AFTER INSERT ON StatisticsAfterRunSurf
+BEGIN
+ UPDATE StatisticsAfterRunSurf SET timeEnter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE INDEX IDX_Map_SS_VW_SignatureId on Map_SS_VW (signatureId);
+CREATE INDEX IDX_Map_SS_VW_VisualWordId on Map_SS_VW (visualWordId);
+CREATE INDEX IDX_Signature_Id on Signature (id);
+CREATE INDEX IDX_VisualWord_Id on VisualWord (id);
+CREATE INDEX IDX_Signature_TimeEnter on Signature (timeEnter);
+CREATE INDEX IDX_VisualWord_TimeEnter on VisualWord (timeEnter);
+CREATE INDEX IDX_Neighbor_Sid on Neighbor (sid);
+
+-- *******************************************************************
+-- Data
+-- *******************************************************************
+INSERT INTO SignatureType(type) VALUES ('fourier');
+INSERT INTO SignatureType(type) VALUES ('surf');
+
+-- *******************************************************************
+-- TESTS
+-- *******************************************************************
+-- *** Data Test ***
+/*
+INSERT INTO Signature VALUES(1, 'surf', null, null, null);
+INSERT INTO Signature VALUES(2, 'surf', null, null, null);
+INSERT INTO Signature VALUES(3, 'surf', null, null, null);
+INSERT INTO VisualWord VALUES (1, 1, 2,'0.213213 0.4352323', null);
+INSERT INTO VisualWord VALUES (2, 1, 2,'0.213213 0.4352323', null);
+INSERT INTO VisualWord VALUES (3, 3, 2,'0.213213 0.4352323', null);
+INSERT INTO Map_SS_VW VALUES (1, 1, 0,0,0,0,0, null);
+INSERT INTO Map_SS_VW VALUES (2, 1, 0,0,0,0,0, null);
+INSERT INTO Map_SS_VW VALUES (2, 2, 0,0,0,0,0, null);
+*/
+
+/*
+-- For loading words
+SELECT vw.id, vw.laplacian, vw.descriptorSize, vw.descriptor, m.signatureId FROM VisualWord as vw INNER JOIN Map_SS_VW as m on vw.id=m.visualWordId ORDER BY vw.id;
+*/
+
+
+-- Refreshing the dictionary
+/*SELECT * FROM Map_SS_VW;
+SELECT * FROM VisualWord;*/
+/*
+DELETE FROM VisualWord;
+INSERT INTO VisualWord VALUES (1, 1, 2,'0.213213 0.4352323', null);
+DELETE FROM Map_SS_VW WHERE NOT EXISTS (SELECT id FROM VisualWord WHERE id = Map_SS_VW.visualWordId);
+*/
+/*SELECT * FROM Map_SS_VW;
+SELECT * FROM VisualWord;*/
+
+/*
+-- Loading only signatures on the last short time memory based on DATE
+INSERT INTO Signature VALUES(4, 'surf', null, null, null);
+INSERT INTO Signature VALUES(5, 'surf', 4, null, null);
+INSERT INTO Signature VALUES(6, 'surf', null, null, null);
+INSERT INTO Map_SS_VW VALUES (4, 1, 0,0,0,0,0, null);
+SELECT s.id FROM Signature AS s WHERE s.timeEnter >= (SELECT vw.timeEnter FROM VisualWord AS vw LIMIT 1) AND s.loopClosureId IS NULL;
+*/