Merged Audio branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@560 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-01 17:10:26 +08:00 · 2012-06-24 17:19:34 +00:00
parent 06fb556e78
commit 17b8e10ed8
111 changed files with 8370 additions and 9779 deletions
--- a/corelib/include/rtabmap/core/CameraEvent.h
+++ b/corelib/include/rtabmap/core/CameraEvent.h
@@ -1,52 +1,53 @@
-/*
- * 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 CAMERAEVENT_H_
-#define CAMERAEVENT_H_
-
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include "utilite/UEvent.h"
-#include <opencv2/core/core.hpp>
-
-namespace rtabmap
-{
-
-class RTABMAP_EXP CameraEvent :
-	public UEvent
-{
-public:
-	enum Code {
-		kCodeNoMoreImages
-	};
-
-public:
-	CameraEvent() :
-		UEvent(kCodeNoMoreImages)
-	{
-	}
-
-	virtual ~CameraEvent() {}
-
-	virtual std::string getClassName() const {return std::string("CameraEvent");}
-};
-
-} // namespace rtabmap
-
-#endif /* CAMERAEVENT_H_ */
+/*
+ * 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 ACTUATOR_H_
+#define ACTUATOR_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <utilite/UEvent.h>
+#include <list>
+
+namespace rtabmap {
+
+class Actuator
+{
+public:
+	enum Type{kTypeTwist=0, kTypeNotSpecified};
+public:
+	Actuator(const cv::Mat & data, Type type, int num = 0) :
+		_data(data),
+		_type(type),
+		_num(num)
+	{}
+	const cv::Mat & data() const {return _data;}
+	int type() const {return _type;}
+	int num() const {return _num;}
+	virtual ~Actuator() {};
+private:
+	cv::Mat _data;
+	int _type;
+	int _num;
+};
+
+}
+
+#endif /* ACTUATOR_H_ */
--- a/corelib/include/rtabmap/core/BayesFilter.h
+++ b/corelib/include/rtabmap/core/BayesFilter.h
@@ -0,0 +1,71 @@
+/*
+ * 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 isPredictionOnNonNullActionsOnly() const {return _predictionOnNonNullActionsOnly;}
+
+	bool generatePrediction(cv::Mat & prediction, const Memory * memory, const std::vector<int> & ids) const;
+
+private:
+	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+	float addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const;
+
+private:
+	std::map<int, float> _posterior;
+	float _virtualPlacePrior;
+	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
+	bool _predictionOnNonNullActionsOnly;
+};
+
+} // namespace rtabmap
+
+#endif /* BAYESFILTER_H_ */
--- a/corelib/include/rtabmap/core/Camera.h
+++ b/corelib/include/rtabmap/core/Camera.h
@@ -22,60 +22,69 @@
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines

 #include <opencv2/highgui/highgui.hpp>
-#include "utilite/UThreadNode.h"
-#include "utilite/UEventsHandler.h"
+#include <opencv2/features2d/features2d.hpp>
+#include <utilite/UThreadNode.h>
+#include <utilite/UEventsHandler.h>
+#include <utilite/UEvent.h>
+#include <utilite/UDirectory.h>
+#include <utilite/UTimer.h>
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/KeypointDetector.h"
+#include "rtabmap/core/KeypointDescriptor.h"
 #include <set>
 #include <stack>
 #include <list>
 #include <vector>

-class UDirectory;
-
 namespace rtabmap
 {

-class KeypointDetector;
-class KeypointDescriptor;
-class SMState;
-
-/**
- * No treatment
- */
-class RTABMAP_EXP CamPostTreatment
+class CameraEvent :
+	public UEvent
 {
 public:
-	CamPostTreatment(const ParametersMap & parameters = ParametersMap()) {
-		this->parseParameters(parameters);
-	}
-	virtual ~CamPostTreatment() {}
-	virtual void process(SMState * smState) const;
-	virtual void parseParameters(const ParametersMap & parameters) {}
-};
-
-/**
- * Extract keypoints from the image
- */
-class RTABMAP_EXP CamKeypointTreatment : public CamPostTreatment
-{
-public:
-	enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
-	enum DescriptorStrategy {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
+	enum Code {
+		kCodeFeatures,
+		kCodeImage,
+		kCodeNoMoreImages
+	};

 public:
-	CamKeypointTreatment(const ParametersMap & parameters = ParametersMap()) :
-		_keypointDetector(0),
-		_keypointDescriptor(0)
+	CameraEvent(const cv::Mat & image, int cameraId = 0) :
+		UEvent(kCodeImage),
+		_cameraId(cameraId),
+		_image(image)
 	{
-		this->parseParameters(parameters);
 	}
-	virtual ~CamKeypointTreatment();
-	virtual void process(SMState * smState) const;
-	virtual void parseParameters(const ParametersMap & parameters);
-	DetectorStrategy detectorStrategy() const;
+	CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int cameraId = 0) :
+		UEvent(kCodeFeatures),
+		_cameraId(cameraId),
+		_image(image),
+		_descriptors(descriptors),
+		_keypoints(keypoints)
+	{
+	}
+	CameraEvent(int cameraId = 0) :
+		UEvent(kCodeNoMoreImages),
+		_cameraId(cameraId)
+	{
+	}
+
+	int cameraId() const {return _cameraId;}
+
+	// Image or descriptors
+	const cv::Mat & image() const {return _image;}
+	const cv::Mat & descriptors() const {return _descriptors;}
+	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
+
+	virtual ~CameraEvent() {}
+	virtual std::string getClassName() const {return std::string("CameraEvent");}
+
 private:
-	KeypointDetector * _keypointDetector;
-	KeypointDescriptor * _keypointDescriptor;
+	int _cameraId;
+	cv::Mat _image;
+	cv::Mat _descriptors;
+	std::vector<cv::KeyPoint> _keypoints;
 };

 /**
@@ -91,17 +100,20 @@ public:

 public:
 	virtual ~Camera();
-
-	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0) = 0;
-	SMState * takeSMState();
+	cv::Mat takeImage();
+	cv::Mat takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & keypoints);
 	virtual bool init() = 0;
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
 	unsigned int getImageWidth() const {return _imageWidth;}
 	unsigned int getImageHeight() const {return _imageHeight;}
-	void setPostThreatement(CamPostTreatment * strategy); // ownership is transferred
+	float getImageRate() const {return _imageRate;}
+	bool isFeaturesExtracted() const {return _featuresExtracted;}
+	void setFeaturesExtracted(bool featuresExtracted, KeypointDetector::DetectorType detector = KeypointDetector::kDetectorUndef, KeypointDescriptor::DescriptorType descriptor = KeypointDescriptor::kDescriptorUndef);
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
+	virtual void parseParameters(const ParametersMap & parameters);
+	int id() const {return _id;}

 protected:
 	/**
@@ -109,15 +121,16 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, bool autoRestart = false, unsigned int imageWidth = 0, unsigned int imageHeight = 0);
+	Camera(float imageRate = 0, bool autoRestart = false, unsigned int imageWidth = 0, unsigned int imageHeight = 0, unsigned int framesDropped = 0, int id = 0);

 	virtual void handleEvent(UEvent* anEvent);
+	virtual cv::Mat captureImage() = 0;

 private:
+	virtual void mainLoopBegin();
 	virtual void mainLoop();
 	void process();
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
-	virtual void parseParameters(const ParametersMap & parameters) {_postThreatement->parseParameters(parameters);}

 private:
 	float _imageRate;
@@ -125,11 +138,16 @@ private:
 	bool _autoRestart;
 	unsigned int _imageWidth;
 	unsigned int _imageHeight;
-	CamPostTreatment * _postThreatement;
+	unsigned int _framesDropped;
+	UTimer _frameRateTimer;

 	UMutex _stateMutex;
 	std::stack<State> _state;
 	std::stack<ParametersMap> _stateParam;
+
+	bool _featuresExtracted;
+	KeypointDetector * _keypointDetector;
+	KeypointDescriptor * _keypointDescriptor;
 };


@@ -147,19 +165,23 @@ public:
 			float imageRate = 0,
 			bool autoRestart = false,
 			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
+			unsigned int imageHeight = 0,
+			unsigned int framesDropped = 0,
+			int id = 0);
 	virtual ~CameraImages();

-	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
 	virtual bool init();

+protected:
+	virtual cv::Mat captureImage();
+
 private:
 	std::string _path;
 	int _startAt;
 	// If the list of files in the directory is refreshed
 	// on each call of takeImage()
 	bool _refreshDir;
-	UDirectory * _dir;
+	UDirectory _dir;
 	int _count;
 	std::string _lastFileName;

@@ -184,22 +206,29 @@ public:
 			float imageRate = 0,
 			bool autoRestart = false,
 			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
+			unsigned int imageHeight = 0,
+			unsigned int framesDropped = 0,
+			int id = 0);
 	CameraVideo(const std::string & fileName,
 			float imageRate = 0,
 			bool autoRestart = false,
 			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
+			unsigned int imageHeight = 0,
+			unsigned int framesDropped = 0,
+			int id = 0);
 	virtual ~CameraVideo();

-	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
 	virtual bool init();
+	int getUsbDevice() {return _usbDevice;}
+
+protected:
+	virtual cv::Mat captureImage();

 private:
 	// File type
 	std::string _fileName;

-	CvCapture* _capture;
+	cv::VideoCapture _capture;
 	Source _src;

 	// Usb camera
@@ -207,36 +236,4 @@ private:
 };


-
-
-
-
-/////////////////////////
-// CameraDatabase
-/////////////////////////
-class DBDriver;
-class RTABMAP_EXP CameraDatabase :
-	public Camera
-{
-public:
-	CameraDatabase(const std::string & path,
-			bool loadActions,
-			float imageRate = 0,
-			bool autoRestart = false,
-			unsigned int imageWidth = 0,
-			unsigned int imageHeight = 0);
-	virtual ~CameraDatabase();
-
-	virtual IplImage * takeImage(std::list<std::vector<float> > * actions = 0);
-	virtual bool init();
-
-private:
-	std::string _path;
-	bool _loadActions;
-	std::set<int>::iterator _indexIter;
-	DBDriver * _dbDriver;
-	std::set<int> _ids;
-};
-
-
 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/ColorTable.h
+++ b/corelib/include/rtabmap/core/ColorTable.h
@@ -0,0 +1,52 @@
+
+#ifndef COLORTABLE_H
+#define COLORTABLE_H
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <vector>
+
+namespace rtabmap
+{
+class RTABMAP_EXP ColorTable
+{
+public:
+	enum Size{kSize8 = 8,
+			kSize16 = 16,
+			kSize32 = 32,
+			kSize64 = 64,
+			kSize128 = 128,
+			kSize256 = 256,
+			kSize512 = 512,
+			kSize1024 = 1024,
+			kSize65536 = 65536};
+public:
+	ColorTable(int size);
+	virtual ~ColorTable() {}
+
+	static unsigned char INDEXED_TABLE_8[24];
+	static unsigned char INDEXED_TABLE_16[48];
+	static unsigned char INDEXED_TABLE_32[96];
+	static unsigned char INDEXED_TABLE_64[192];
+	static unsigned char INDEXED_TABLE_128[384];
+	static unsigned char INDEXED_TABLE_256[768];
+	static unsigned char INDEXED_TABLE_512[1536];
+	static unsigned char INDEXED_TABLE_1024[3076];
+	static unsigned char INDEXED_TABLE_65536[196608];
+
+	int size() const {return _size;}
+	unsigned short getIndex(unsigned char r, unsigned char g, unsigned char b) const;
+	void getRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
+
+	unsigned short getNNIndex(unsigned char r, unsigned char g, unsigned char b) const;
+	void getNNRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
+
+private:
+	int _size;
+	std::vector<unsigned short> _rgb2indexed;
+	unsigned char * _indexedTable;
+};
+
+} // namespace rtabmap
+
+#endif // COLORTABLE_H
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -92,22 +92,23 @@ public:

 	// Load objects
 	bool load(VWDictionary * dictionary) const;
-	bool loadLastSignatures(std::list<Signature *> & signatures) const;
+	bool loadLastNodes(std::list<Signature *> & signatures) const;
 	bool loadKeypointSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
 	bool loadSMSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
 	bool loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws);

 	// Specific queries...
-	bool getImage(int id, IplImage ** img) const;
-	bool getNeighborIds(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) const;
+	bool getRawData(int id, std::list<Sensor> & data) const;
+	bool getActuatorData(int id, std::list<Actuator> & data) const;
+	bool getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
 	bool loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const;
 	bool getWeight(int signatureId, int & weight) const;
 	bool getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
-	bool getAllSignatureIds(std::set<int> & ids) const;
-	bool getLastSignatureId(int & id) const;
-	bool getLastVisualWordId(int & id) const;
-	bool getSurfNi(int signatureId, int & ni) const;
-	bool getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const;
+	bool getAllNodeIds(std::set<int> & ids) const;
+	bool getLastNodeId(int & id) const;
+	bool getLastWordId(int & id) const;
+	bool getInvertedIndexNi(int signatureId, int & ni) const;
+	bool getHighestWeightedNodeIds(unsigned int count, std::multimap<int, int> & ids) const;

 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -122,7 +123,7 @@ private:

 	virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const = 0; // <oldWordId, activeWordId>
 	virtual bool deleteWordsQuery(const std::vector<int> & ids) const = 0;
-	virtual bool getNeighborIdsQuery(int signatureId, std::list<int> & neighbors, bool onlyWithActions = false) const = 0;
+	virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const = 0;
 	virtual bool getWeightQuery(int signatureId, int & weight) const = 0;
 	virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const = 0;

@@ -132,7 +133,7 @@ private:

 	// Load objects
 	virtual bool loadQuery(VWDictionary * dictionary) const = 0;
-	virtual bool loadLastSignaturesQuery(std::list<Signature *> & signatures) const = 0;
+	virtual bool loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual bool loadQuery(int signatureId, Signature ** s) const = 0;
 	virtual bool loadQuery(int wordId, VisualWord ** vw) const = 0;
 	virtual bool loadQuery(int signatureId, KeypointSignature * ss) const = 0;
@@ -142,12 +143,13 @@ private:
 	virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	virtual bool loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const = 0;

-	virtual bool getImageQuery(int id, IplImage ** image) const = 0;
-	virtual bool getAllSignatureIdsQuery(std::set<int> & ids) const = 0;
-	virtual bool getLastSignatureIdQuery(int & id) const = 0;
-	virtual bool getLastVisualWordIdQuery(int & id) const = 0;
-	virtual bool getSurfNiQuery(int signatureId, int & ni) const = 0;
-	virtual bool getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int,int> & signatures) const = 0;
+	virtual bool getRawDataQuery(int id, std::list<Sensor> & rawData) const = 0;
+	virtual bool getActuatorDataQuery(int id, std::list<Actuator> & rawData) const = 0;
+	virtual bool getAllNodeIdsQuery(std::set<int> & ids) const = 0;
+	virtual bool getLastNodeIdQuery(int & id) const = 0;
+	virtual bool getLastWordIdQuery(int & id) const = 0;
+	virtual bool getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
+	virtual bool getHighestWeightedNodeIdsQuery(unsigned int count, std::multimap<int,int> & signatures) const = 0;

 private:
 	//non-abstract methods
@@ -167,7 +169,6 @@ private:
 	unsigned int _minSignaturesToSave;
 	unsigned int _minWordsToSave;
 	bool _imagesCompressed;
-	bool _asyncWaiting;
 	double _emptyTrashesTime;
 	std::string _url;
 };
--- a/corelib/include/rtabmap/core/DBReader.h
+++ b/corelib/include/rtabmap/core/DBReader.h
@@ -0,0 +1,54 @@
+/*
+ * DBReader.h
+ *
+ *  Created on: 2012-06-13
+ *      Author: mathieu
+ */
+
+#ifndef DBREADER_H_
+#define DBREADER_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/Sensor.h"
+#include "rtabmap/core/Actuator.h"
+
+#include <utilite/UThreadNode.h>
+#include <utilite/UTimer.h>
+
+#include <set>
+
+namespace rtabmap {
+
+class DBDriver;
+
+class RTABMAP_EXP DBReader : public UThreadNode {
+public:
+	DBReader(const std::string & databasePath,
+			 float frameRate = 0.0f,
+			 const std::set<Sensor::Type> & sensorTypes = std::set<Sensor::Type>(),
+			 const std::set<Actuator::Type> & actuatorTypes = std::set<Actuator::Type>());
+	virtual ~DBReader();
+
+	bool init();
+	void setFrameRate(float frameRate);
+	void getNextSensorimotorState(std::list<Sensor> & sensors, std::list<Actuator> & actuators);
+
+protected:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+private:
+	std::string _path;
+	float _frameRate;
+	std::set<Sensor::Type> _sensorTypes;
+	std::set<Actuator::Type> _actuatorTypes;
+
+	DBDriver * _dbDriver;
+	UTimer _timer;
+	std::set<int> _ids;
+	std::set<int>::iterator _currentId;
+};
+
+} /* namespace rtabmap */
+#endif /* DBREADER_H_ */
--- a/corelib/include/rtabmap/core/EpipolarGeometry.h
+++ b/corelib/include/rtabmap/core/EpipolarGeometry.h
@@ -22,12 +22,63 @@
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines

 #include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include <vector>

 namespace rtabmap
 {

 //epipolar geometry
-void RTABMAP_EXP findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2);
-void RTABMAP_EXP findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2 = cv::Vec3d());
+void RTABMAP_EXP findEpipolesFromF(
+		const cv::Mat & fundamentalMatrix,
+		cv::Vec3d & e1,
+		cv::Vec3d & e2);
+
+cv::Mat RTABMAP_EXP findPFromF(
+		const cv::Mat & fundamentalMatrix,
+		const cv::Mat & x1,
+		const cv::Mat & x2);
+
+// return fundamental matrix
+// status -> inliers = 1, outliers = 0
+cv::Mat RTABMAP_EXP findFFromWords(
+		const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
+		std::vector<uchar> & status,
+		double ransacParam1 = 3.0,
+		double ransacParam2 = 0.99);
+
+// assume a canonical camera (without K)
+void RTABMAP_EXP findRTFromP(
+		const cv::Mat & p,
+		cv::Mat & r,
+		cv::Mat & t);
+
+/**
+ * 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 RTABMAP_EXP findPairs(
+		const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
+ * realPairsCount = 5
+ */
+int RTABMAP_EXP findPairsUnique(
+		const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);
+
+/**
+ * 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 RTABMAP_EXP findPairsAll(
+		const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs);

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/KeypointDescriptor.h
+++ b/corelib/include/rtabmap/core/KeypointDescriptor.h
@@ -31,10 +31,13 @@
 namespace rtabmap {

 class RTABMAP_EXP KeypointDescriptor {
+public:
+	enum DescriptorType {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
+
 public:
 	virtual ~KeypointDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const = 0;
+	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;

 protected:
 	KeypointDescriptor(const ParametersMap & parameters = ParametersMap());
@@ -47,10 +50,15 @@ public:
 	SURFDescriptor(const ParametersMap & parameters = ParametersMap());
 	virtual ~SURFDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;

 private:
-	CvSURFParams _params;
+	double _hessianThreshold;
+	int _nOctaves;
+	int _nOctaveLayers;
+	bool _extended;
+	bool _upright;
+
 	bool _gpuVersion;
 };

@@ -61,11 +69,14 @@ public:
 	SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
 	virtual ~SIFTDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;

 private:
-	cv::SIFT::CommonParams _commonParams;
-	cv::SIFT::DescriptorParams _descriptorParams;
+	int _nfeatures;
+	int _nOctaveLayers;
+	double _contrastThreshold;
+	double _edgeThreshold;
+	double _sigma;
 };

 //BRIEFDescriptor
@@ -75,7 +86,7 @@ public:
 	BRIEFDescriptor(const ParametersMap & parameters = ParametersMap());
 	virtual ~BRIEFDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;

 private:
 	int _size;
@@ -88,7 +99,7 @@ public:
 	ColorDescriptor(const ParametersMap & parameters = ParametersMap());
 	virtual ~ColorDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 protected:
 	void getCircularROI(int R, std::vector<int> & RxV) const;
 };
@@ -100,7 +111,7 @@ public:
 	HueDescriptor(const ParametersMap & parameters = ParametersMap());
 	virtual ~HueDescriptor();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const IplImage * image, std::vector<cv::KeyPoint> & keypoints) const;
+	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 private:
 	// assuming that rgb values are normalized [0,1]
 	float rgb2hue(float r, float g, float b) const;
--- a/corelib/include/rtabmap/core/KeypointDetector.h
+++ b/corelib/include/rtabmap/core/KeypointDetector.h
@@ -35,25 +35,22 @@ class VWDictionary;

 class RTABMAP_EXP KeypointDetector
 {
+public:
+	enum DetectorType {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
+
 public:
 	virtual ~KeypointDetector() {}
-	std::vector<cv::KeyPoint> generateKeypoints(const IplImage * image);
+	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image);
 	virtual void parseParameters(const ParametersMap & parameters);
 	unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
-	double getAdaptiveResponseThr() const {return _adaptiveResponseThr;}
-	virtual double getMinimumResponseThr() const = 0;
-	bool isUsingAdaptiveResponseThr() const {return _usingAdaptiveResponseThr;}
 	void setRoi(const std::string & roi);
-	cv::Rect computeRoi(const IplImage * image) const;
+	cv::Rect computeRoi(const cv::Mat & image) const;
 protected:
 	KeypointDetector(const ParametersMap & parameters = ParametersMap());
-	void setAdaptiveResponseThr(float adaptiveResponseThr) {_adaptiveResponseThr = adaptiveResponseThr;}
 private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const = 0;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
 private:
 	unsigned int _wordsPerImageTarget;
-	bool _usingAdaptiveResponseThr;
-	double _adaptiveResponseThr;
 	std::vector<float> _roiRatios; // size 4
 };

@@ -64,11 +61,15 @@ public:
 	SURFDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~SURFDetector();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual double getMinimumResponseThr() const {return _params.hessianThreshold;};
 private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
 private:
-	CvSURFParams _params;
+	double _hessianThreshold;
+	int _nOctaves;
+	int _nOctaveLayers;
+	bool _extended;
+	bool _upright;
+
 	bool _gpuVersion;
 };

@@ -79,12 +80,14 @@ public:
 	SIFTDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~SIFTDetector();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual double getMinimumResponseThr() const {return _detectorParams.threshold;};
 private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
 private:
-	cv::SIFT::CommonParams _commonParams;
-	cv::SIFT::DetectorParams _detectorParams;
+	int _nfeatures;
+	int _nOctaveLayers;
+	double _contrastThreshold;
+	double _edgeThreshold;
+	double _sigma;
 };

 //StarDetector
@@ -94,11 +97,14 @@ public:
 	StarDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~StarDetector();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual double getMinimumResponseThr() const {return (double)_params.responseThreshold;};
 private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
 private:
-	CvStarDetectorParams _params;
+	int _maxSize;
+	int _responseThreshold;
+	int _lineThresholdProjected;
+	int _lineThresholdBinarized;
+	int _suppressNonmaxSize;
 };

 //FASTDetector
@@ -108,9 +114,8 @@ public:
 	FASTDetector(const ParametersMap & parameters = ParametersMap());
 	virtual ~FASTDetector();
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual double getMinimumResponseThr() const {return (double)_threshold;};
 private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
+	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
 private:
 	int _threshold;
 	bool _nonmaxSuppression;
--- a/corelib/include/rtabmap/core/KeypointMemory.h
+++ b/corelib/include/rtabmap/core/KeypointMemory.h
@@ -0,0 +1,86 @@
+/*
+ * 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 "rtabmap/core/Memory.h"
+#include <opencv2/features2d/features2d.hpp>
+
+namespace rtabmap {
+
+class VWDictionary;
+class VisualWord;
+class KeypointDetector;
+class KeypointDescriptor;
+
+class RTABMAP_EXP KeypointMemory : public Memory
+{
+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::list<int> & ids, float & maximumScore);
+	virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
+	virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
+	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;}
+	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
+
+protected:
+	virtual Signature * getSignatureLtMem(int id);
+	virtual void addSignatureToStm(Signature * signature, const std::list<Actuator> & actions = std::list<Actuator>());
+	virtual void clear();
+	virtual void moveToTrash(Signature * s);
+	virtual void preUpdate();
+
+private:
+	virtual void copyData(const Signature * from, Signature * to);
+	virtual Signature * createSignature(int id, const std::list<Sensor> & sensors, bool keepRawData=false);
+
+	void disableWordsRef(int signatureId);
+	void enableWordsRef(const std::list<int> & signatureIds);
+	void cleanUnusedWords();
+	int getNi(int signatureId) const;
+
+private:
+	VWDictionary * _vwd;
+	KeypointDetector * _keypointDetector;
+	KeypointDescriptor * _keypointDescriptor;
+	//std::map<int, int> _wordRefsToChange;
+	bool _reactivatedWordsComparedToNewWords;
+	float _badSignRatio;;
+	bool _tfIdfLikelihoodUsed;
+	bool _parallelized;
+	bool _tfIdfNormalized;
+};
+
+}
+
+#endif /* KEYPOINTMEMORY_H_ */
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -0,0 +1,194 @@
+/*
+ * 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>
+#include "rtabmap/core/Sensor.h"
+#include "rtabmap/core/Actuator.h"
+
+namespace rtabmap {
+
+class Signature;
+class NeighborLink;
+class DBDriver;
+class Node;
+
+class RTABMAP_EXP Memory
+{
+public:
+	static const int kIdStart;
+	static const int kIdVirtual;
+	static const int kIdInvalid;
+
+public:
+	Memory(const ParametersMap & parameters = ParametersMap());
+	virtual ~Memory();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	bool update(const std::list<Sensor> & sensors,
+			const std::list<Actuator> & actuators,
+			std::map<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::list<int> & ids,
+			float & maximumScore);
+	virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
+	virtual std::set<int> reactivateSignatures(const std::list<int> & ids,
+			unsigned int maxLoaded,
+			double & timeDbAccess);
+
+	int cleanup(const std::list<int> & ignoredIds = std::list<int>());
+	void emptyTrash();
+	void joinTrashThread();
+	bool addLoopClosureLink(int oldId, int newId);
+	std::map<int, int> getNeighborsId(double & dbAccessTime,
+			int signatureId,
+			unsigned int margin,
+			int maxCheckedInDatabase = -1,
+			bool onlyWithActions = false,
+			bool incrementMarginOnLoop = false,
+			bool ignoreSTM = true,
+			bool ignoreLoopIds = false) const;
+	float compareOneToOne(const std::vector<int> & idsA, const std::vector<int> & idsB);
+
+	//getters
+	unsigned int getWorkingMemSize() const {return _workingMem.size();}
+	unsigned int getStMemSize() const {return _stMem.size();};
+	const std::set<int> & getWorkingMem() const {return _workingMem;}
+	const std::set<int> & getStMem() const {return _stMem;}
+	std::list<NeighborLink> getNeighborLinks(int signatureId,
+			bool ignoreNeighborByLoopClosure = false,
+			bool lookInDatabase = false,
+			bool onlyWithActions = false) const;
+	void getLoopClosureIds(int signatureId,
+			std::set<int> & loopClosureIds,
+			std::set<int> & childLoopClosureIds,
+			bool lookInDatabase = false) const;
+	bool isRawDataKept() const {return _rawDataKept;}
+	float getSimilarityThr() const {return _similarityThreshold;}
+	std::map<int, int> getWeights() const;
+	int getWeight(int id) const;
+	const std::vector<int> & getLastBaseIds() const {return _lastBaseIds;}
+	float getSimilarityOnlyLast() const {return _similarityOnlyLast;}
+	const std::map<int, std::map<int, float> > & getSimilaritiesMap() const {return _similaritiesMap;}
+	const Signature * getLastSignature() const;
+	int getDatabaseMemoryUsed() const; // in bytes
+	double getDbSavingTime() const;
+	std::list<Sensor> getRawData(int id) const;
+	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 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 addSignatureToStm(Signature * signature,
+			const std::list<Actuator> & actuators = std::list<Actuator>());
+	virtual void clear();
+	virtual void moveToTrash(Signature * s);
+	virtual Signature * getSignatureLtMem(int id);
+
+	void addSignatureToWm(Signature * signature);
+	Signature * _getSignature(int id) const;
+	std::list<Signature *> getRemovableSignatures(int count,
+			const std::set<int> & ignoredIds = std::set<int>());
+	int getNextId();
+	void initCountId();
+	void rehearsal(Signature * signature, std::map<std::string, float> & stats);
+
+	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
+
+private:
+	virtual void copyData(const Signature * from, Signature * to) = 0;
+	virtual Signature * createSignature(int id,
+			const std::list<Sensor> & sensors,
+			bool keepRawData=false) = 0;
+
+	void createVirtualSignature(Signature ** signature);
+	void cleanGraph(const Node * root);
+protected:
+	DBDriver * _dbDriver;
+
+private:
+	// parameters
+	float _similarityThreshold;
+	bool _similarityOnlyLast;
+	bool _rawDataKept;
+	bool _incrementalMemory;
+	unsigned int _maxStMemSize;
+	bool _commonSignatureUsed;
+	float _recentWmRatio;
+	bool _dataMergedOnRehearsal;
+
+	int _idCount;
+	Signature * _lastSignature;
+	int _lastLoopClosureId;
+	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; // id
+	std::set<int> _workingMem; // id,age
+	std::vector<int> _lastBaseIds;
+	std::map<int, std::map<int, float> > _similaritiesMap;
+};
+
+} // namespace rtabmap
+
+#endif /* MEMORY_H_ */
--- a/corelib/include/rtabmap/core/Micro.h
+++ b/corelib/include/rtabmap/core/Micro.h
@@ -0,0 +1,134 @@
+/*
+ * Micro.h
+ *
+ *  Created on: Mar 5, 2012
+ *      Author: MatLab
+ */
+
+#ifndef MICRO_H_
+#define MICRO_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <utilite/UThreadNode.h>
+#include <utilite/UTimer.h>
+#include <utilite/UEvent.h>
+#include <utilite/ULogger.h>
+#include <string>
+#include <vector>
+#include <opencv2/core/core.hpp>
+
+class UAudioRecorder;
+
+namespace rtabmap {
+
+class MicroEvent :
+	public UEvent
+{
+public:
+	enum Type {
+		kTypeFrame,
+		kTypeFrameFreq,
+		kTypeFrameFreqSqrdMagn,
+		kTypeNoMoreFrames
+	};
+
+public:
+	// kTypeNoMoreFrames constructor
+	MicroEvent(int microId = 0) :
+		UEvent(kTypeNoMoreFrames),
+		_sampleSize(0),
+		_microId(microId)
+	{
+	}
+	// kTypeFrame constructor
+	MicroEvent(const cv::Mat & frame,
+			int sampleSize,
+			int fs,
+			int channels,
+			int microId = 0) :
+		UEvent(kTypeFrame),
+		_frame(frame),
+		_sampleSize(sampleSize),
+		_microId(microId)
+	{
+	}
+	// kTypeFrameFreq and kTypeFrameFreqSqrdMagn constructors
+	MicroEvent(Type frameType,
+			const cv::Mat & frameFreq,
+			int fs,
+			int channels,
+			int microId = 0) :
+		UEvent(frameType),
+		_frame(frameFreq),
+		_sampleSize(sizeof(float)),
+		_microId(microId)
+	{
+		UASSERT(frameType == kTypeFrameFreqSqrdMagn || frameType == kTypeFrameFreq);
+	}
+
+	int type() const {return this->getCode();}
+	const cv::Mat & frame() const {return _frame;}
+	int sampleSize() const {return _sampleSize;}
+	int microId() const {return _microId;}
+	virtual ~MicroEvent() {}
+	virtual std::string getClassName() const {return std::string("MicroEvent");}
+
+private:
+	cv::Mat _frame;
+	int _sampleSize; // bytes
+	int _fs; //sampling rate
+	int _microId;
+};
+
+class RTABMAP_EXP Micro : public UThreadNode
+{
+	typedef float fftwf_complex[2];
+public:
+	Micro(MicroEvent::Type eventType,
+			int deviceId,
+			int fs,
+			int frameLength,
+			int channels,
+			int bytesPerSample,
+			int id = 0);
+	Micro(MicroEvent::Type eventType,
+			const std::string & path,
+			bool simulateFrameRate,
+			int frameLength,
+			int id = 0,
+			bool playWhileRecording = false);
+	virtual ~Micro();
+
+	bool init();
+	void stop(); // same as kill() but handle the case where underlying recorder is running and not the micro.
+	void startRecorder(); // must only be used if Micro::start() is not used
+
+	cv::Mat getFrame();
+	cv::Mat getFrame(cv::Mat & frameFreq, bool sqrdMagn = false);
+
+	int fs();
+	int bytesPerSample();
+	int channels();
+	int nfft();
+
+protected:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+	virtual void mainLoopKill();
+
+private:
+	MicroEvent::Type _eventType;
+	UAudioRecorder* _recorder;
+	bool _simulateFreq;
+	UTimer _timer;
+	std::vector<float> _window;
+	std::vector<float> _in;
+	fftwf_complex * _out; // fftwf_complex
+	void * _p; // fftwf_plan
+	int _id;
+};
+
+}
+
+#endif /* MICRO_H_ */
--- a/corelib/include/rtabmap/core/NearestNeighbor.h
+++ b/corelib/include/rtabmap/core/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
+// FIXME KdTreeNN seems broken, it does not give same results as naive and FLANN
+/////////////////////////
+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 false;}
+	virtual bool isDistSquared() const {return false;}
+	virtual void parseParameters(const ParametersMap & parameters);
+private:
+	cv::KDTree _tree;
+};
+
+
+
+
+
+
+/////////////////////////
+// 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_ */
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -122,13 +122,13 @@ class RTABMAP_EXP Parameters
 	// Rtabmap parameters
 	RTABMAP_PARAM(Rtabmap, VhStrategy, 	         int, 0);	   // None 0, Similarity 1, Epipolar 2
 	RTABMAP_PARAM(Rtabmap, PublishStats, 	     bool, true); // Publishing statistics
-	RTABMAP_PARAM(Rtabmap, PublishImages, 	     bool, true); // Publishing images
+	RTABMAP_PARAM(Rtabmap, PublishRawData, 	     bool, true); // Publishing raw data
 	RTABMAP_PARAM(Rtabmap, PublishPdf, 	         bool, true); // Publishing pdf
 	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	 bool, true); // Publishing likelihood
 	RTABMAP_PARAM(Rtabmap, RetrievalThr,         float, 0.0); // Reactivation threshold
 	RTABMAP_PARAM(Rtabmap, TimeThr, 		     float, 700.0); // Maximum time allowed for the detector (ms) (0 means infinity)
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		     int, 0); // Maximum signatures in the Working Memory (ms) (0 means infinity)
-	RTABMAP_PARAM(Rtabmap, SMStateBufferSize,    int, 1);      // Data buffer size (0 min inf)
+	RTABMAP_PARAM(Rtabmap, SMStateBufferSize,    int, 0);      // Data buffer size (0 min inf)
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory());	   // Working directory
 	RTABMAP_PARAM(Rtabmap, MaxRetrieved,       unsigned int, 2); // Maximum locations retrieved at the same time from LTM
 	RTABMAP_PARAM(Rtabmap, SelectionNeighborhoodSummationUsed, bool, false); // Neighborhood summation for hypothesis selection
@@ -136,15 +136,16 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, ActionsSentRejectHyp,  bool, true); // Actions sent also on rejected hypotheses (on decreasing hypotheses)
 	RTABMAP_PARAM(Rtabmap, ConfidenceThr, 		float, 0.0); // Actions are not sent when the loop closure hypothesis is under the confidence threshold
 	RTABMAP_PARAM(Rtabmap, LikelihoodStdDevRemoved, bool, true); // Remove std dev on likelihood normalization.
+	RTABMAP_PARAM(Rtabmap, LikelihoodNullValuesIgnored, bool, true); // Ignore null values on likelihood normalization

 	// Hypotheses selection
 	RTABMAP_PARAM(Rtabmap, LoopThr,    	     float, 0.15); // Loop closing threshold
-	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.0); // The loop closure hypothesis must be over LoopRatio x lastHypothesisValue
+	RTABMAP_PARAM(Rtabmap, LoopRatio,    	 float, 0.9); // The loop closure hypothesis must be over LoopRatio x lastHypothesisValue

 	// Memory
 	RTABMAP_PARAM(Mem, SimilarityThr, 	       float, 0.20);  // Similarity between the last signature and neighbor
 	RTABMAP_PARAM(Mem, SimilarityOnlyLast,     bool, false);	// Only compare to the last signature in STM, otherwise it compares to all signatures in STM
-	RTABMAP_PARAM(Mem, RawDataKept, 		   bool, false); // Keep raw data
+	RTABMAP_PARAM(Mem, RawDataKept, 		   bool, true); // Keep raw data
 	RTABMAP_PARAM(Mem, MaxStMemSize, 		   unsigned int, 30); // Short-time memory size
 	RTABMAP_PARAM(Mem, CommonSignatureUsed,    bool, true); // A common signature/virtual place is automatically updated with id -1
 	RTABMAP_PARAM(Mem, IncrementalMemory, 	   bool, true);
@@ -165,7 +166,6 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, MaxLeafs, 	    int, 64);  // Maximum number of leafs checked (when using kd-trees)
 	RTABMAP_PARAM(Kp, DetectorStrategy, int, 0);      // Surf detector 0, Star detector 1, SIFT detector 2, FAST detector 3
 	RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0);      // kDescriptorSurf=0, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef
-	RTABMAP_PARAM(Kp, UsingAdaptiveResponseThr, bool, false);
 	RTABMAP_PARAM(Kp, ReactivatedWordsComparedToNewWords, bool, true); //Reactivated words are compared to the last words added in the dictionary (which are not indexed)
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false); // Use of the td-idf strategy to compute the likelihood
 	RTABMAP_PARAM(Kp, Parallelized, bool, true); // If the dictionary update and signature creation were parallelized
@@ -174,11 +174,13 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM_STR(Kp, DictionaryPath, ""); // Path of the pre-computed dictionary

 	// SM memory
-	RTABMAP_PARAM(SM, PublishMasks, 	 bool, true); // Publishing motion masks
+	RTABMAP_PARAM(SM, PublishMasks, 	 bool, false); // Publishing motion masks
 	RTABMAP_PARAM(SM, MotionMaskUsed, bool, false); // Use motion mask
 	RTABMAP_PARAM(SM, LogPolarUsed, bool, false); // Use log-polar images
-	RTABMAP_PARAM(SM, VotingSchemeUsed, bool, false); // Use likelihood voting scheme
 	RTABMAP_PARAM(SM, ColorTable, int, 8); // Color table size 0=8, 1=16, 2=32, 3=64, 4=128, 5=256, 6=512, 7=1024, 8=65536
+	RTABMAP_PARAM(SM, AudioDBThreshold, float, 0.0f); // Audio dB threshold
+	RTABMAP_PARAM(SM, AudioDBIndexing, bool, true); // dB (decibel) indexing (otherwise it's squared magnitude indexing)
+	RTABMAP_PARAM(SM, MagnitudeInvariant, bool, false); // Make audio signature magnitude-invariant

 	//Database
 	RTABMAP_PARAM(Db, MinSignaturesToSave, int, 20);	 // Minimum signatures needed in the trash to save them (empty trash thread)
@@ -195,11 +197,14 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(SURF, HessianThreshold, float, 150.0);
 	RTABMAP_PARAM(SURF, Octaves, 		  int, 4);
 	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2);
-	RTABMAP_PARAM(SURF, GpuVersion, 	  bool, false);
 	RTABMAP_PARAM(SURF, Upright, 	      bool, false); // U-SURF
+	RTABMAP_PARAM(SURF, GpuVersion, 	  bool, false);

-	RTABMAP_PARAM(SIFT, Threshold, 	double, 0.006667); // true=128, false=64
+	RTABMAP_PARAM(SIFT, NFeatures, int, 0);
+	RTABMAP_PARAM(SIFT, NOctaveLayers, int, 3);
+	RTABMAP_PARAM(SIFT, ContrastThreshold, 	double, 0.04);
 	RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
+	RTABMAP_PARAM(SIFT, Sigma, double, 1.6);

 	RTABMAP_PARAM(FAST, Threshold, 	int, 10);
 	RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true);
--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -17,8 +17,8 @@
 * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
 */

-#ifndef CTABMAP_H_
-#define CTABMAP_H_
+#ifndef RTABMAP_H_
+#define RTABMAP_H_

 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines

@@ -29,6 +29,8 @@
 #include "utilite/UVariant.h"
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/Sensor.h"
+#include "rtabmap/core/Actuator.h"
 #include <opencv2/core/core.hpp>
 #include <list>
 #include <stack>
@@ -42,7 +44,6 @@ class Signature;
 class HypVerificator;
 class Memory;
 class BayesFilter;
-class SMState;

 class RTABMAP_EXP Rtabmap :
 	public UThreadNode,
@@ -57,7 +58,8 @@ public:
 		kStateDumpingMemory,
 		kStateDumpingPrediction,
 		kStateGeneratingGraph,
-		kStateDeletingMemory
+		kStateDeletingMemory,
+		kStateCleanSensorsBuffer
 	};

 	enum VhStrategy {kVhNone, kVhSim, kVhEpipolar, kVhUndef};
@@ -76,18 +78,19 @@ public:
 	Rtabmap();
 	virtual ~Rtabmap();

-	// ownership is transferred
-	void process(SMState * data);
+	void process(const std::list<Sensor> & data);
+	void process(const Sensor & data); // for convenience when only one sensor is used
 	void dumpData();

 	void init(const ParametersMap & param);
 	void init(const char * configFile = 0);
+	void clearBufferedSensors();

 	const std::string & getWorkingDir() const {return _wDir;}
 	int getLoopClosureId() const;
 	int getReactivatedId() const;
 	int getLastSignatureId() const;
-	const std::list<std::vector<float> > & getActions() const {return _actions;}
+	const std::list<Actuator> & getActuator() const {return _actuators;}
 	std::list<int> getWorkingMem() const;
 	std::set<int> getStMem() const;
 	std::map<int, int> getWeights() const;
@@ -110,12 +113,12 @@ protected:

 private:
 	virtual void mainLoop();
-	virtual void killCleanup();
-	virtual void startInit();
+	virtual void mainLoopKill();
+	virtual void mainLoopBegin();
 	void process();
 	void resetMemory(bool dbOverwritten = false);
-	void addSMState(SMState * data); // ownership is transferred
-	SMState * getSMState();
+	void addSensorimotor(const std::list<Sensor> & sensors, const std::list<Actuator> & actuators);
+	void getSensorimotor(std::list<Sensor> & sensors, std::list<Actuator> & actuators);
 	void setupLogFiles(bool overwrite = false);
 	void releaseAllStrategies();
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
@@ -125,14 +128,14 @@ private:
 private:
 	// Modifiable parameters
 	bool _publishStats;
-	bool _publishImages;
+	bool _publishRawData;
 	bool _publishPdf;
 	bool _publishLikelihood;
 	bool _publishKeypoints;
 	bool _publishMasks;
 	float _maxTimeAllowed; // in ms
 	unsigned int _maxMemoryAllowed; // signatures count in WM
-	int _smStateBufferMaxSize;
+	int _sensorsBufferMaxSize;
 	float _loopThr;
 	float _loopRatio;
 	float _retrievalThr;
@@ -142,20 +145,21 @@ private:
 	bool _actionsSentRejectHyp;
 	float _confidenceThr;
 	bool _likelihoodStdDevRemoved;
+	bool _likelihoodNullValuesIgnored;

 	int _lcHypothesisId;
 	int _reactivateId;
 	float _lastLcHypothesisValue;
 	int _lastLoopClosureId;
-	std::list<std::vector<float> > _actions;
+	std::list<Actuator> _actuators;

 	UMutex _stateMutex;
 	std::stack<State> _state;
 	std::stack<ParametersMap> _stateParam;

-	std::list<SMState *> _smStateBuffer;
-	UMutex _smStateBufferMutex;
-	USemaphore _newSMStateSem;
+	std::list<std::pair<std::list<Sensor>, std::list<Actuator> > > _sensorimotorBuffer;
+	UMutex _sensorimotorMutex;
+	USemaphore _sensorimotorAdded;

 	// Abstract classes containing all loop closure
 	// strategies for a type of signature or configuration.
@@ -171,6 +175,6 @@ private:
 	std::string _graphFileName;
 };

-#endif /* CTABMAP_H_ */
+#endif /* RTABMAP_H_ */

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/RtabmapEvent.h
+++ b/corelib/include/rtabmap/core/RtabmapEvent.h
@@ -22,11 +22,13 @@

 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines

-#include "utilite/UEvent.h"
+#include "rtabmap/core/Sensor.h"
+#include "rtabmap/core/Actuator.h"
+#include <utilite/UEvent.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
-#include "utilite/ULogger.h"
+#include <utilite/ULogger.h>
 #include <list>
 #include <vector>

@@ -63,6 +65,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Signatures_removed,);
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
+	RTABMAP_STATS(Memory, Similarities_map,);

 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
@@ -90,7 +93,6 @@ public:

 public:
 	Statistics();
-	Statistics(const Statistics & s);
 	virtual ~Statistics();

 	// name format = "Grp/Name/unit"
@@ -100,11 +102,9 @@ public:
 	void setExtended(bool extended) {_extended = extended;}
 	void setRefImageId(int refImageId) {_refImageId = refImageId;}
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
-	void setActions(const std::list<std::vector<float> > & actions) {_actions = actions;}
-	void setRefImage(IplImage ** refImage);
-	void setRefImage(const IplImage * refImage);
-	void setLoopClosureImage(IplImage ** loopClosureImage);
-	void setLoopClosureImage(const IplImage * loopClosureImage);
+	void setActuators(const std::list<Actuator> & actuators) {_actuators = actuators;}
+	void setRefRawData(const std::list<Sensor> & refRawData);
+	void setLoopClosureRawData(const std::list<Sensor> & loopClosureRawData);
 	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
 	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
 	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
@@ -117,9 +117,9 @@ public:
 	bool extended() const {return _extended;}
 	int refImageId() const {return _refImageId;}
 	int loopClosureId() const {return _loopClosureId;}
-	const std::list<std::vector<float> > & getActions() const {return _actions;}
-	const IplImage * refImage() const {return _refImage;}
-	const IplImage * loopClosureImage() const {return _loopClosureImage;}
+	const std::list<Actuator> & getActuators() const {return _actuators;}
+	const std::list<Sensor> & refRawData() const {return _refRawData;}
+	const std::list<Sensor> & loopClosureRawData() const {return _loopClosureRawData;}
 	const std::map<int, int> & weights() const {return _weights;}
 	const std::map<int, float> & posterior() const {return _posterior;}
 	const std::map<int, float> & likelihood() const {return _likelihood;}
@@ -130,20 +130,17 @@ public:

 	const std::map<std::string, float> & data() const {return _data;}

-
-	Statistics & operator=(const Statistics & s);
-
 private:
 	int _extended; // 0 -> only loop closure and last signature ID fields are filled

 	int _refImageId;
 	int _loopClosureId;

-	std::list<std::vector<float> > _actions;
+	std::list<Actuator> _actuators;

 	// extended data start here...
-	IplImage * _refImage; // Released by the event destructor
-	IplImage * _loopClosureImage; // Released by the event destructor
+	std::list<Sensor> _refRawData;
+	std::list<Sensor> _loopClosureRawData;

 	std::map<int, int> _weights;
 	std::map<int, float> _posterior;
@@ -191,7 +188,8 @@ public:
 		kCmdDumpMemory,
 		kCmdDumpPrediction,
 		kCmdGenerateGraph,
-		kCmdDeleteMemory};
+		kCmdDeleteMemory,
+		kCmdCleanSensorsBuffer};
 public:
 	RtabmapEventCmd(Cmd cmd) :
 		UEvent(0),
--- a/corelib/include/rtabmap/core/RtabmapExp.h
+++ b/corelib/include/rtabmap/core/RtabmapExp.h
@@ -20,16 +20,12 @@
 #ifndef RTABMAPEXP_H
 #define RTABMAPEXP_H

-#ifdef WIN32
-  #ifdef RTABMAP_EXPORTS
+#if defined(_WIN32)
+  #if defined(rtabmap_corelib_EXPORTS) || defined(rtabmap_guilib_EXPORTS)
    #define RTABMAP_EXP   __declspec( dllexport )
  #else
-    #ifdef RTABMAP_EXPORTS_STATIC
-    #define RTABMAP_EXP
-    #else
-      #define RTABMAP_EXP   __declspec( dllimport )
-    #endif
-  #endif
+    #define RTABMAP_EXP   __declspec( dllimport )
+  #endif
 #else
  #define RTABMAP_EXP
 #endif
--- a/corelib/include/rtabmap/core/SMMemory.h
+++ b/corelib/include/rtabmap/core/SMMemory.h
@@ -0,0 +1,57 @@
+/*
+ * 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 SIMPLEMEMORY_H_
+#define SIMPLEMEMORY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include "rtabmap/core/Memory.h"
+
+namespace rtabmap {
+
+class ColorTable;
+class SMSignature;
+
+class RTABMAP_EXP SMMemory : public Memory
+{
+public:
+	SMMemory(const ParametersMap & parameters = ParametersMap());
+	virtual ~SMMemory();
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
+	void setRoi(const std::string & roi);
+	void setColorTable(int size);
+
+private:
+	virtual void copyData(const Signature * from, Signature * to);
+	virtual Signature * createSignature(int id, const std::list<Sensor> & sensors, bool keepRawData=false);
+
+private:
+	bool _useLogPolar;
+	ColorTable * _colorTable;
+	bool _useMotionMask;
+	float _dBThreshold;
+	bool _dBIndexing;
+	bool _magnitudeInvariant;
+};
+
+}
+
+#endif /* KEYPOINTMEMORY_H_ */
--- a/corelib/include/rtabmap/core/SMState.h
+++ b/corelib/include/rtabmap/core/SMState.h
@@ -1,157 +0,0 @@
-/*
- * 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 SMPAIRVARIANT_H_
-#define SMPAIRVARIANT_H_
-
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui_c.h>
-#include <opencv2/features2d/features2d.hpp>
-#include <list>
-#include <vector>
-#include <utilite/ULogger.h>
-
-namespace rtabmap {
-
-// SensoriMotor state
-class SMState
-{
-public:
-	// Constructor 0 : ownership is transferred
-	SMState(IplImage * image = 0) :
-		_image(image)
-	{}
-	// Constructor 1
-	SMState(const cv::Mat & sensors, const std::list<std::vector<float> > & actuators) :
-		_sensors(sensors),
-		_actuators(actuators),
-		_image(0)
-	{}
-	// Constructor 2 : for convenience with ROS conversion...
-	// Sensors and actuators vectors will be split into a list with smaller vectors of length sensorStep and actuatorStep respectively.
-	SMState(const std::vector<float> & sensors, int sensorStep, const std::vector<float> & actuators, int actuatorStep) :
-		_image(0)
-	{
-		if(sensorStep && sensors.size() % sensorStep != 0)
-		{
-			UERROR("Sensors must all have the same length.");
-		}
-		if(actuatorStep && actuators.size() % actuatorStep != 0)
-		{
-			UERROR("Actuators must all have the same length.");
-		}
-		for(unsigned int i=0; i<sensors.size() && i<i+sensorStep; i+=sensorStep)
-		{
-			_sensors.push_back(std::vector<float>(sensors.data()+i, sensors.data()+i+sensorStep));
-		}
-		for(unsigned int i=0; i<actuators.size() && i<i+actuatorStep; i+=actuatorStep)
-		{
-			_actuators.push_back(std::vector<float>(actuators.data()+i, actuators.data()+i+actuatorStep));
-		}
-	}
-
-	virtual ~SMState()
-	{
-		if(_image)
-		{
-			cvReleaseImage(&_image);
-		}
-	}
-
-	const IplImage * getImage() const {return _image;}
-	const std::vector<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
-	const cv::Mat & getSensors() const {return _sensors;}
-	const std::list<std::vector<float> > & getActuators() const {return _actuators;}
-	void setSensors(const cv::Mat & sensors) {_sensors=sensors;}
-	void setActuators(const std::list<std::vector<float> > & actuators) {_actuators=actuators;}
-	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
-
-	//ownership is transferred
-	void setImage(IplImage * image)
-	{
-		if(_image)
-		{
-			cvReleaseImage(&_image);
-		}
-		_image = image;
-	}
-
-	void getSensorsMerged(std::vector<float> & sensors, int & step) const
-	{
-		sensors.clear();
-		step = 0;
-		if(!_sensors.empty())
-		{
-			// here we assume that all sensors have the same length
-			step = _sensors.cols;
-			sensors = std::vector<float>(_sensors.total());
-			for(int i=0; i<_sensors.rows; ++i)
-			{
-				const float * rowFl = _sensors.ptr<float>(i);
-				memcpy(&sensors[i*_sensors.cols], rowFl, _sensors.cols*sizeof(float));
-			}
-		}
-	}
-	void getActuatorsMerged(std::vector<float> & actuators, int & step) const
-	{
-		actuators.clear();
-		step = 0;
-		if(_actuators.size())
-		{
-			// here we assume that all sensors have the same length
-			step = _actuators.front().size();
-			for(std::list<std::vector<float> >::const_iterator iter = _actuators.begin();
-					iter != _actuators.end();
-					++iter)
-			{
-				actuators.insert(actuators.end(), iter->begin(), iter->end());
-			}
-		}
-	}
-
-private:
-	cv::Mat _sensors; // descriptors
-	std::list<std::vector<float> > _actuators;
-	IplImage * _image;
-	std::vector<cv::KeyPoint> _keypoints;
-};
-
-// Sensorimotor state event
-// Take ownership of the state
-class SMStateEvent : public UEvent
-{
-public:
-	SMStateEvent(SMState * state) :
-		UEvent(0),
-		_state(state) {}
-
-	virtual ~SMStateEvent() {if(_state) delete _state;}
-	const SMState * getSMState() const {return _state;}
-	SMState * getSMStateOwnership() {SMState * state = _state; _state=0; return state;}
-	virtual std::string getClassName() const {return "SMStateEvent";} // TODO : macro?
-
-private:
-	SMState * _state;
-};
-
-}
-
-#endif /* SMPAIRVARIANT_H_ */
--- a/corelib/include/rtabmap/core/Sensor.h
+++ b/corelib/include/rtabmap/core/Sensor.h
@@ -0,0 +1,64 @@
+/*
+ * 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 SENSOR_H_
+#define SENSOR_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <vector>
+#include <list>
+#include <utilite/UEvent.h>
+
+namespace rtabmap {
+
+class Sensor
+{
+public:
+	enum Type{kTypeImage=0, kTypeImageFeatures2d, kTypeAudio, kTypeAudioFreq, kTypeAudioFreqSqrdMagn, kTypeJointState, kTypeTwist, kTypeNotSpecified};
+public:
+	Sensor(const cv::Mat & data, Type type, int num = 0) :
+		_data(data),
+		_type(type),
+		_num(num)
+	{}
+	Sensor(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, int num = 0) :
+		_data(descriptors),
+		_type(kTypeImageFeatures2d),
+		_num(num),
+		_keypoints(keypoints)
+	{}
+	const cv::Mat & data() const {return _data;}
+	int type() const {return _type;}
+	int num() const {return _num;}
+	virtual ~Sensor() {};
+	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
+	const std::vector<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
+private:
+	cv::Mat _data;
+	int _type;
+	int _num; // sensor number
+	std::vector<cv::KeyPoint> _keypoints; // for convenience with kTypeImageFeatures
+};
+
+}
+
+#endif /* SENSOR_H_ */
--- a/corelib/include/rtabmap/core/SensorimotorEvent.h
+++ b/corelib/include/rtabmap/core/SensorimotorEvent.h
@@ -0,0 +1,47 @@
+/*
+ * SensorimotorEvent.h
+ *
+ *  Created on: 2012-05-27
+ *      Author: mathieu
+ */
+
+#ifndef SENSORIMOTOREVENT_H_
+#define SENSORIMOTOREVENT_H_
+
+#include "rtabmap/core/Sensor.h"
+#include "rtabmap/core/Actuator.h"
+#include <utilite/UEvent.h>
+
+namespace rtabmap
+{
+
+class SensorimotorEvent : public UEvent
+{
+public:
+	enum Type {
+			kTypeData,
+			kTypeNoMoreData
+		};
+
+public:
+	SensorimotorEvent() :
+		UEvent(kTypeNoMoreData) {}
+	SensorimotorEvent(const std::list<Sensor> & sensors,
+					  const std::list<Actuator> & actuators) :
+        UEvent(kTypeData),
+		sensors_(sensors),
+		actuators_(actuators) {}
+	virtual ~SensorimotorEvent() {}
+	int type() const {return this->getCode();}
+	virtual std::string getClassName() const {return "SensorimotorEvent";}
+	const std::list<Sensor> & sensors() const {return sensors_;}
+	const std::list<Actuator> & actuators() const {return actuators_;}
+private:
+	std::list<Sensor> sensors_;
+	std::list<Actuator> actuators_;
+};
+
+}
+
+
+#endif /* SENSORIMOTOREVENT_H_ */
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -24,6 +24,8 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include "rtabmap/core/Sensor.h"
+#include "rtabmap/core/Actuator.h"
 #include <map>
 #include <list>
 #include <vector>
@@ -37,21 +39,24 @@ namespace rtabmap
 class RTABMAP_EXP NeighborLink
 {
 public:
-	NeighborLink(int id, const std::list<std::vector<float> > & actions = std::list<std::vector<float> >(), const std::vector<int> & baseIds = std::vector<int>()) :
-		_id(id),
-		_actions(actions),
+	NeighborLink(int toId, const std::vector<int> & baseIds = std::vector<int>(), const std::list<Actuator> & actuators = std::list<Actuator>(), int actuatorId = 0) :
+		_toId(toId),
+		_actuatorId(actuatorId),
+		_actuators(actuators),
 		_baseIds(baseIds)
 	{}
 	virtual ~NeighborLink() {}

-	int id() const {return _id;}
-	const std::list<std::vector<float> > & actions() const {return _actions;}
+	int toId() const {return _toId;}
+	int actuatorId() const {return _actuatorId;}
+	const std::list<Actuator> & actuators() const {return _actuators;}
 	const std::vector<int> & baseIds() const {return _baseIds;}
 	bool updateIds(int idFrom, int idTo);

 private:
-	int _id;
-	std::list<std::vector<float> > _actions;
+	int _toId;
+	int _actuatorId;
+	std::list<Actuator> _actuators;
 	std::vector<int> _baseIds; // first is the nearest
 };

@@ -60,10 +65,6 @@ typedef std::multimap<int, NeighborLink> NeighborsMultiMap;

 class RTABMAP_EXP Signature
 {
-public:
-	static CvMat * compressImage(const IplImage * image);
-	static IplImage * decompressImage(const CvMat * imageCompressed);
-
 public:
 	virtual ~Signature();

@@ -72,29 +73,35 @@ public:
 	 */
 	virtual float compareTo(const Signature * signature) const = 0;
 	virtual bool isBadSignature() const = 0;
-	virtual std::string signatureType() const = 0;
+	virtual std::string nodeType() const = 0;

-	const IplImage * getImage() const;
-	void setImage(const IplImage * image);
+	void setRawData(const std::list<Sensor> & rawData) {_rawData = rawData;}
+	const std::list<Sensor> & getRawData() const {return _rawData;}

 	int id() const {return _id;}

 	void addNeighbors(const NeighborsMultiMap & neighbors);
 	void addNeighbor(const NeighborLink & neighbor);
-	void removeNeighbor(int neighborId) {if(_neighbors.erase(neighborId)) _neighborsModified = true;}
+	void removeNeighbor(int neighborId) {
+		if(_neighbors.erase(neighborId))
+			_neighborsModified = true;
+		_neighborsWithActuators.erase(neighborId);
+		_neighborsAll.erase(neighborId);}
 	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
 	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
-	void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_modified=true;}
-	void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_modified=true;}
-	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_modified=true;}
+	void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
+	void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_neighborsModified=true;}
+	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
 	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
-	void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_modified=true;}
-	void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_modified=true;}
+	void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
+	void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_neighborsModified=true;}
 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
 	void changeNeighborIds(int idFrom, int idTo);

 	const NeighborsMultiMap & getNeighbors() const {return _neighbors;}
+	const std::set<int> & getNeighborsWithActuators() const {return _neighborsWithActuators;}
+	const std::set<int> & getNeighborsAll() const {return _neighborsAll;}
 	int getWeight() const {return _weight;}
 	const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
 	const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
@@ -103,15 +110,18 @@ public:
 	bool isNeighborsModified() const {return _neighborsModified;}

 protected:
-	Signature(int id, const IplImage * image = 0, bool keepImage = false);
+	Signature(int id);
+	Signature(int id, const std::list<Sensor> & rawData);

 private:
 	int _id;
 	NeighborsMultiMap _neighbors; // id, neighborLink
+	std::set<int> _neighborsWithActuators; // Hack, to increase efficiency of Memory::getNeighborIds()
+	std::set<int> _neighborsAll; // Hack, to increase efficiency of Memory::getNeighborIds()
 	int _weight;
 	std::set<int> _loopClosureIds;
 	std::set<int> _childLoopClosureIds;
-	IplImage * _image;
+	std::list<Sensor> _rawData;
 	bool _saved; // If it's saved to bd
 	bool _modified;
 	bool _neighborsModified; // Optimization when updating signatures in database
@@ -125,18 +135,20 @@ class RTABMAP_EXP KeypointSignature :
 	public Signature
 {
 public:
+	KeypointSignature(int id);
+	KeypointSignature(
+			const std::multimap<int, cv::KeyPoint> & words,
+			int id);
 	KeypointSignature(
 			const std::multimap<int, cv::KeyPoint> & words,
 			int id,
-			const IplImage * image = 0,
-			bool keepRawData = false);
-	KeypointSignature(int id);
+			const std::list<Sensor> & sensors);

 	virtual ~KeypointSignature();

 	virtual float compareTo(const Signature * signature) const;
 	virtual bool isBadSignature() const;
-	virtual std::string signatureType() const {return "KeypointSignature";};
+	virtual std::string nodeType() const {return "KeypointSignature";};

 	void removeAllWords();
 	void removeWord(int wordId);
@@ -164,27 +176,24 @@ class RTABMAP_EXP SMSignature :
 {
 public:
 	SMSignature(
-		const std::vector<int> & sensors,
-		const std::vector<unsigned char> & motionMask,
+		const std::list<std::vector<int> > & data,
+		int id);
+	SMSignature(
+		const std::list<std::vector<int> > & data,
 		int id,
-		const IplImage * image = 0,
-		bool keepRawData = false);
+		const std::list<Sensor> & rawData);
 	SMSignature(int id);

 	virtual ~SMSignature();

 	virtual float compareTo(const Signature * signature) const;
 	virtual bool isBadSignature() const;
-	virtual std::string signatureType() const {return "SMSignature";};
+	virtual std::string nodeType() const {return "SMSignature";};

-	void setSensors(const std::vector<int> & sensors) {_sensors= sensors;}
-	const std::vector<int> & getSensors() const {return _sensors;}
-
-	void setMotionMask(const std::vector<unsigned char> & motionMask) {_motionMask= motionMask;}
-	const std::vector<unsigned char> & getMotionMask() const {return _motionMask;}
+	void setSensors(const std::list<std::vector<int> > & data) {_data = data;}
+	const std::list<std::vector<int> > & getData() const {return _data;}
 private:
-	std::vector<int> _sensors;
-	std::vector<unsigned char> _motionMask;
+	std::list<std::vector<int> > _data;
 };


--- a/corelib/include/rtabmap/core/VWDictionary.h
+++ b/corelib/include/rtabmap/core/VWDictionary.h
@@ -0,0 +1,115 @@
+/*
+ * 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/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 VisualWord;
+
+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 cv::Mat & descriptors,
+			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, 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;
+	const VisualWord * getUnusedWord(int id) const;
+	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)
+	int _dim;
+	int _lastWordId;
+	NearestNeighbor * _nn;
+	cv::Mat _dataTree;
+	std::map<int ,int> _mapIndexId;
+	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
+};
+
+} // namespace rtabmap
--- a/corelib/include/rtabmap/core/VerifyHypotheses.h
+++ b/corelib/include/rtabmap/core/VerifyHypotheses.h
@@ -0,0 +1,96 @@
+/*
+ * 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 Signature;
+
+// return always true, i.e, there is no verification
+class RTABMAP_EXP HypVerificator
+{
+public:
+	HypVerificator(const ParametersMap & parameters = ParametersMap());
+	virtual ~HypVerificator() {}
+	virtual bool verify(const Signature * ref, const Signature * hyp);
+
+	virtual void parseParameters(const ParametersMap & parameters);
+};
+
+
+/////////////////////////
+// HypVerificatorSim
+/////////////////////////
+class HypVerificatorSim : public HypVerificator {
+public:
+	HypVerificatorSim(const ParametersMap & parameters = ParametersMap());
+	virtual ~HypVerificatorSim();
+	virtual bool verify(const Signature * ref, const Signature * hyp);
+	virtual void parseParameters(const ParametersMap & parameters);
+
+private:
+	float _similarity;
+};
+
+
+/////////////////////////
+// HypVerificatorEpipolarGeo
+/////////////////////////
+class KeypointSignature;
+
+class RTABMAP_EXP HypVerificatorEpipolarGeo : public HypVerificator
+{
+public:
+	HypVerificatorEpipolarGeo(const ParametersMap & parameters = ParametersMap());
+	virtual ~HypVerificatorEpipolarGeo();
+	virtual bool verify(const Signature * ref, const Signature * hyp);
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	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;}
+
+private:
+	bool doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB);
+
+private:
+	int _matchCountMinAccepted;
+	double _ransacParam1;
+	double _ransacParam2;
+};
+
+} // namespace rtabmap
+
+#endif /* VERIFYHYPOTHESES_H_ */
--- a/corelib/include/rtabmap/core/VisualWord.h
+++ b/corelib/include/rtabmap/core/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, 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;}
+	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;
+	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