Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-02 01:20:25 +08:00 · 2013-12-11 00:12:44 +00:00
parent 97c70d394e
commit 8b8511e154
124 changed files with 21692 additions and 4458 deletions
--- a/corelib/include/rtabmap/core/Camera.h
+++ b/corelib/include/rtabmap/core/Camera.h
@@ -66,7 +66,6 @@ public:


 	virtual void parseParameters(const ParametersMap & parameters);
-	int id() const {return _id;}

 protected:
 	/**
@@ -74,13 +73,15 @@ protected:
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, unsigned int imageWidth = 0, unsigned int imageHeight = 0, unsigned int framesDropped = 0, int id = 0);
+	Camera(float imageRate = 0,
+			unsigned int imageWidth = 0,
+			unsigned int imageHeight = 0,
+			unsigned int framesDropped = 0);

 	virtual cv::Mat captureImage() = 0;

 private:
 	float _imageRate;
-	int _id;
 	unsigned int _imageWidth;
 	unsigned int _imageHeight;
 	unsigned int _framesDropped;
@@ -105,8 +106,7 @@ public:
 			float imageRate = 0,
 			unsigned int imageWidth = 0,
 			unsigned int imageHeight = 0,
-			unsigned int framesDropped = 0,
-			int id = 0);
+			unsigned int framesDropped = 0);
 	virtual ~CameraImages();

 	virtual bool init();
@@ -143,14 +143,12 @@ public:
 			float imageRate = 0,
 			unsigned int imageWidth = 0,
 			unsigned int imageHeight = 0,
-			unsigned int framesDropped = 0,
-			int id = 0);
+			unsigned int framesDropped = 0);
 	CameraVideo(const std::string & filePath,
 			float imageRate = 0,
 			unsigned int imageWidth = 0,
 			unsigned int imageHeight = 0,
-			unsigned int framesDropped = 0,
-			int id = 0);
+			unsigned int framesDropped = 0);
 	virtual ~CameraVideo();

 	virtual bool init();
--- a/corelib/include/rtabmap/core/CameraEvent.h
+++ b/corelib/include/rtabmap/core/CameraEvent.h
@@ -34,29 +34,35 @@ public:
 	enum Code {
 		kCodeFeatures,
 		kCodeImage,
+		kCodeImageDepth,
 		kCodeNoMoreImages
 	};

 public:
-	CameraEvent(const cv::Mat & image, int cameraId = 0) :
+	CameraEvent(const cv::Mat & image, int seq=0) :
 		UEvent(kCodeImage),
-		_cameraId(cameraId),
-		_image(image)
+		_image(image, seq)
 	{
 	}
-	CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int cameraId = 0) :
+	CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int seq=0) :
 		UEvent(kCodeFeatures),
-		_cameraId(cameraId),
-		_image(image, 0, descriptors, keypoints)
+		_image(image, seq, descriptors, keypoints)
 	{
 	}
-	CameraEvent(int cameraId = 0) :
-		UEvent(kCodeNoMoreImages),
-		_cameraId(cameraId)
+	CameraEvent() :
+		UEvent(kCodeNoMoreImages)
+	{
+	}
+	CameraEvent(const cv::Mat & image, const cv::Mat & depth, float depthConstant, const Transform & localTransform, int seq=0) :
+		UEvent(kCodeImageDepth),
+		_image(image, depth, depthConstant, Transform(), localTransform, seq)
+	{
+	}
+	CameraEvent(const cv::Mat & image, const cv::Mat & depth, const cv::Mat & depth2d, float depthConstant, const Transform & localTransform, int seq=0) :
+		UEvent(kCodeImageDepth),
+		_image(image, depth, depth2d, depthConstant, Transform(), localTransform, seq)
 	{
 	}
-
-	int cameraId() const {return _cameraId;}

 	// Image or descriptors
 	const Image & image() const {return _image;}
@@ -65,7 +71,6 @@ public:
 	virtual std::string getClassName() const {return std::string("CameraEvent");}

 private:
-	int _cameraId;
 	Image _image;
 };

--- a/corelib/include/rtabmap/core/CameraOpenni.h
+++ b/corelib/include/rtabmap/core/CameraOpenni.h
@@ -0,0 +1,63 @@
+/*
+ * CameraOpenni.h
+ *
+ *  Created on: 2013-08-22
+ *      Author: Mathieu
+ */
+
+#ifndef CAMERAOPENNI_H_
+#define CAMERAOPENNI_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <pcl/io/openni_camera/openni_depth_image.h>
+#include <pcl/io/openni_camera/openni_image.h>
+
+#include <boost/signals2/connection.hpp>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UEventsSender.h>
+
+class UTimer;
+
+namespace pcl
+{
+	class Grabber;
+}
+
+namespace rtabmap {
+
+class RTABMAP_EXP CameraOpenni : public UEventsSender
+{
+public:
+	// default local transform z in, x right, y down));
+	CameraOpenni(const std::string & deviceId="",
+			float rate=0,
+			const Transform & localTRansform = Transform::getIdentity());
+	virtual ~CameraOpenni();
+
+    void image_cb (
+    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
+			float constant);
+
+    bool init();
+    void start();
+    void pause();
+    void kill();
+    bool isRunning();
+
+    void setFrameRate(float rate);
+
+private:
+    pcl::Grabber* interface_;
+    std::string deviceId_;
+    float rate_;
+    UTimer * frameRateTimer_;
+    Transform localTransform_; // transform from camera_optical_link to base_link
+    int seq_;
+    boost::signals2::connection connection_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAOPENNI_H_ */
--- a/corelib/include/rtabmap/core/CameraThread.h
+++ b/corelib/include/rtabmap/core/CameraThread.h
@@ -49,11 +49,13 @@ public:
 	CameraThread(Camera * camera, bool autoRestart = false);
 	virtual ~CameraThread();

+	bool init(); // call camera->init()
+
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
 	void setAutoRestart(bool autoRestart) {_autoRestart = autoRestart;}
-	void setImageRate(float imageRate);
+	Camera * getCamera() {return _camera;}

 protected:
 	virtual void handleEvent(UEvent* anEvent);
@@ -69,6 +71,7 @@ private:
 	std::stack<State> _state;
 	std::stack<ParametersMap> _stateParam;
 	bool _autoRestart;
+	int _seq;
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/corelib/include/rtabmap/core/DBDriver.h
@@ -31,6 +31,8 @@
 #include "rtabmap/utilite/UThreadNode.h"
 #include "rtabmap/core/Parameters.h"

+#include <rtabmap/core/Transform.h>
+
 namespace rtabmap {

 class Signature;
@@ -62,11 +64,9 @@ public:
 	void asyncSave(VisualWord * vw); //ownership transferred
 	void emptyTrashes(bool async = false);
 	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
-	bool isImagesCompressed() const {return _imagesCompressed;}

 public:
-	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const;
-	void addStatisticsAfterRunSurf(int dictionarySize) const;
+	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const;

 public:
 	// Mutex-protected methods of abstract versions below
@@ -82,20 +82,24 @@ public:

 	// Load objects
 	void load(VWDictionary * dictionary) const;
+	void load(std::map<int, std::map<int, Transform> > & mapTransforms) const;
 	void loadLastNodes(std::list<Signature *> & signatures) const;
 	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);

 	// Specific queries...
-	void getImage(int id, cv::Mat & image) const;
-	void getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
-	void loadNeighbors(int signatureId, std::set<int> & neighbors) const;
+	void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
+	void getNodeData(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const;
+	void getNodeData(int signatureId, std::vector<unsigned char> & image) const;
+	void getPose(int signatureId, Transform & pose, int & mapId) const;
+	void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
+	void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const;
 	void getWeight(int signatureId, int & weight) const;
-	void getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
 	void getAllNodeIds(std::set<int> & ids) const;
 	void getLastNodeId(int & id) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
+	void save(const std::map<int, std::map<int, Transform> > & mapTransforms) const;

 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -108,24 +112,28 @@ private:

 	virtual void executeNoResultQuery(const std::string & sql) const = 0;

-	virtual void getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const = 0;
 	virtual void getWeightQuery(int signatureId, int & weight) const = 0;
-	virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const = 0;

 	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
 	virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
 	virtual void updateQuery(const std::list<VisualWord *> & words) const = 0;
+	virtual void saveQuery(const std::map<int, std::map<int, Transform> > & mapTransforms) const = 0;


 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;
+	virtual void loadQuery(std::map<int, std::map<int, Transform> > & mapTransforms) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
-	virtual void loadNeighborsQuery(int signatureId, std::set<int> & neighbors) const = 0;
+	virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const = 0;
+	virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;

-	virtual void getImageQuery(int id, cv::Mat & rawData) const = 0;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
+	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & depthConstant, Transform & localTransform) const = 0;
+	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const = 0;
+	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
@@ -145,7 +153,6 @@ private:
 	UMutex _trashesMutex;
 	UMutex _dbSafeAccessMutex;
 	USemaphore _addSem;
-	bool _imagesCompressed;
 	double _emptyTrashesTime;
 	std::string _url;
 };
--- a/corelib/include/rtabmap/core/DBReader.h
+++ b/corelib/include/rtabmap/core/DBReader.h
@@ -12,6 +12,8 @@

 #include <rtabmap/utilite/UThreadNode.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/core/Transform.h>

 #include <opencv2/core/core.hpp>

@@ -21,15 +23,16 @@ namespace rtabmap {

 class DBDriver;

-class RTABMAP_EXP DBReader : public UThreadNode {
+class RTABMAP_EXP DBReader : public UThreadNode, public UEventsSender {
 public:
 	DBReader(const std::string & databasePath,
-			 float frameRate = 0.0f);
+			 float frameRate = 0.0f,
+			 bool odometryIgnored = false);
 	virtual ~DBReader();

 	bool init(int startIndex=0);
 	void setFrameRate(float frameRate);
-	void getNextImage(cv::Mat & sensors);
+	void getNextImage(cv::Mat & image, cv::Mat & depth, cv::Mat & depth2d, float & depthConstant, Transform & localTransform, Transform & pose);

 protected:
 	virtual void mainLoopBegin();
@@ -38,6 +41,7 @@ protected:
 private:
 	std::string _path;
 	float _frameRate;
+	bool _odometryIgnored;

 	DBDriver * _dbDriver;
 	UTimer _timer;
--- a/corelib/include/rtabmap/core/Features2d.h
+++ b/corelib/include/rtabmap/core/Features2d.h
@@ -30,6 +30,10 @@

 namespace rtabmap {

+
+void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
+void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
+
 /////////////////////
 // KeypointDescriptor
 /////////////////////
@@ -91,20 +95,22 @@ class RTABMAP_EXP KeypointDetector
 public:
 	enum DetectorType {kDetectorSurf, kDetectorSift, kDetectorUndef};

+public:
+	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
+
 public:
 	virtual ~KeypointDetector() {}
-	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image);
+	std::vector<cv::KeyPoint> generateKeypoints(
+			const cv::Mat & image,
+			int maxKeypoints = 0,
+			const cv::Rect & roi = cv::Rect());
 	virtual void parseParameters(const ParametersMap & parameters);
-	unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
 	void setRoi(const std::string & roi);
-	cv::Rect computeRoi(const cv::Mat & image) const;
+
 protected:
 	KeypointDetector(const ParametersMap & parameters = ParametersMap());
 private:
 	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
-private:
-	unsigned int _wordsPerImageTarget;
-	std::vector<float> _roiRatios; // size 4
 };

 //SURFDetector
--- a/corelib/include/rtabmap/core/Image.h
+++ b/corelib/include/rtabmap/core/Image.h
@@ -8,11 +8,11 @@
 #ifndef IMAGE_H_
 #define IMAGE_H_

-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>

+#include <rtabmap/core/Transform.h>
+
 namespace rtabmap
 {

@@ -29,21 +29,76 @@ public:
 		_image(image),
 		_id(id),
 		_descriptors(descriptors),
-		_keypoints(keypoints)
+		_keypoints(keypoints),
+		_depthConstant(0.0f),
+		_localTransform(Transform::getIdentity())
 	{
 	}

+	// Metric constructor
+	Image(const cv::Mat & image,
+		  const cv::Mat & depth,
+		  float depthConstant,
+		  const Transform & pose,
+		  const Transform & localTransform,
+		  int id = 0) :
+		_image(image),
+		_id(id),
+		_depth(depth),
+		_depthConstant(depthConstant),
+		_pose(pose),
+		_localTransform(localTransform)
+	{
+	}
+
+	// Metric constructor + 2d depth
+	Image(const cv::Mat & image,
+		  const cv::Mat & depth,
+		  const cv::Mat & depth2d,
+		  float depthConstant,
+		  const Transform & pose,
+		  const Transform & localTransform,
+		  int id = 0) :
+		_image(image),
+		_id(id),
+		_depth(depth),
+		_depth2d(depth2d),
+		_depthConstant(depthConstant),
+		_pose(pose),
+		_localTransform(localTransform)
+	{
+	}
+
+	virtual ~Image() {}
+
 	bool empty() const {return _image.empty() && _descriptors.empty() && _keypoints.size() == 0;}
 	const cv::Mat & image() const {return _image;}
 	int id() const {return _id;};
 	const cv::Mat & descriptors() const {return _descriptors;}
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
+	void setDescriptors(const cv::Mat & descriptors) {_descriptors = descriptors;}
+	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
+
+	bool isMetric() const {return !_depth.empty() || _depthConstant != 0.0f || !_pose.isNull();}
+	void setPose(const Transform & pose) {_pose = pose;}
+	const cv::Mat & depth() const {return _depth;}
+	const cv::Mat & depth2d() const {return _depth2d;}
+	float depthConstant() const {return _depthConstant;}
+	const Transform & pose() const {return _pose;}
+	const Transform & localTransform() const {return _localTransform;}

 private:
 	cv::Mat _image;
 	int _id;
 	cv::Mat _descriptors;
 	std::vector<cv::KeyPoint> _keypoints;
+
+	// Metric stuff
+	cv::Mat _depth;
+	cv::Mat _depth2d;
+	float _depthConstant;
+	Transform _pose;
+	Transform _localTransform;
 };

 }
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/corelib/include/rtabmap/core/Memory.h
@@ -42,6 +42,7 @@ class VWDictionary;
 class VisualWord;
 class KeypointDetector;
 class KeypointDescriptor;
+class Statistics;

 class RTABMAP_EXP Memory
 {
@@ -55,64 +56,73 @@ public:
 	virtual ~Memory();

 	virtual void parseParameters(const ParametersMap & parameters);
-	bool update(const Image & image, std::map<std::string, float> & stats);
+	bool update(const Image & image, Statistics * stats = 0);
 	bool init(const std::string & dbUrl,
 			bool dbOverwritten = false,
-			const ParametersMap & parameters = ParametersMap());
+			const ParametersMap & parameters = ParametersMap(),
+			bool postInitEvents = true);
 	std::map<int, float> computeLikelihood(const Signature * signature,
 			const std::list<int> & ids);
+	int incrementMapId();

-	int forget(const std::set<int> & ignoredIds = std::set<int>());
+	std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);

-	int cleanup(const std::list<int> & ignoredIds = std::list<int>());
+	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
 	void emptyTrash();
 	void joinTrashThread();
-	bool addLoopClosureLink(int oldId, int newId);
+	bool addLoopClosureLink(int oldId, int newId, const Transform & transform);
+	void updateNeighborLink(int fromId, int toId, const Transform & transform);
 	std::map<int, int> getNeighborsId(int signatureId,
 			unsigned int margin,
 			int maxCheckedInDatabase = -1,
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
-	void deleteLastLocation();
 	void deleteLocation(int locationId);
-	void rejectLastLoopClosure();
+	void rejectLoopClosure(int oldId, int newId);

 	//getters
 	const std::set<int> & getWorkingMem() const {return _workingMem;}
 	const std::set<int> & getStMem() const {return _stMem;}
 	int getMaxStMemSize() const {return _maxStMemSize;}
-	std::set<int> getNeighborLinks(int signatureId,
+	Transform getMapTransform(int sourceMapId, int targetMapId) const;
+	void removeMapTransform(int soureId, int targetId);
+	void getPose(int locationId,
+			int targetMapId,
+			Transform & pose,
+			bool lookInDatabase = false) const;
+	std::map<int, Transform> getNeighborLinks(int signatureId,
 			bool ignoreNeighborByLoopClosure = false,
 			bool lookInDatabase = false) const;
 	void getLoopClosureIds(int signatureId,
-			std::set<int> & loopClosureIds,
-			std::set<int> & childLoopClosureIds,
+			std::map<int, Transform> & loopClosureIds,
+			std::map<int, Transform> & childLoopClosureIds,
 			bool lookInDatabase = false) const;
 	bool isRawDataKept() const {return _rawDataKept;}
 	float getSimilarityThreshold() const {return _similarityThreshold;}
 	std::map<int, int> getWeights() const;
-	float getSimilarityOnlyWithLast() const {return _rehearsalOnlyWithLast;}
 	int getLastSignatureId() const;
 	const Signature * getLastWorkingSignature() const;
 	int getDatabaseMemoryUsed() const; // in bytes
 	double getDbSavingTime() const;
-	cv::Mat getImage(int signatureId) const;
+	std::vector<unsigned char> getImage(int signatureId) const;
+	void getImageDepth(
+			int locationId, std::vector<unsigned char> & rgb,
+			std::vector<unsigned char> & depth,
+			std::vector<unsigned char> & depth2d,
+			float & depthConstant,
+			Transform & localTransform) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
+	bool isIncremental() const {return _incrementalMemory;}
 	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);}
+	bool isIDsGenerated() const {return _generateIds;}

-	//setters
-	void setSimilarityThreshold(float similarity);
-	void setSimilarityOnlyLast(int rehearsalOnlyWithLast) {_rehearsalOnlyWithLast = rehearsalOnlyWithLast;}
-	void setOldSignatureRatio(float oldSignatureRatio);
-	void setMaxStMemSize(unsigned int maxStMemSize);
-	void setRecentWmRatio(float recentWmRatio);
-	void setRawDataKept(bool rawDataKept) {_rawDataKept = rawDataKept;}
+	void setRoi(const std::string & roi);

 	void dumpMemoryTree(const char * fileNameTree) const;
 	virtual void dumpMemory(std::string directory) const;
@@ -127,8 +137,27 @@ public:
 	//keypoint stuff
 	int getVWDictionarySize() const;
 	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
+	void extractKeypointsAndDescriptors(
+			const cv::Mat & image,
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors);

-protected:
+	void getMetricConstraints(
+			const std::vector<int> & ids,
+			int targetMapId,
+			std::map<int, Transform> & poses,
+			std::multimap<int, std::pair<int, Transform> > & links,
+			bool lookInDatabase = false);
+	Transform computeVisualTransform(int oldId, int newId) const;
+	Transform computeVisualTransform(const Signature & oldS, const Signature & newS) const;
+	Transform computeIcpTransform(int oldId, int newId, Transform guess);
+	Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess) const;
+	Transform computeScanMatchingTransform(
+			int newId,
+			int oldId,
+			const std::map<int, Transform> & poses);
+
+private:
 	void preUpdate();
 	void addSignatureToStm(Signature * signature);
 	void clear();
@@ -140,11 +169,11 @@ protected:
 			const std::set<int> & ignoredIds = std::set<int>());
 	int getNextId();
 	void initCountId();
-	void rehearsal(Signature * signature, std::map<std::string, float> & stats);
+	void rehearsal(Signature * signature, Statistics * stats = 0);
+	bool rehearsalMerge(int oldId, int newId);

 	const std::map<int, Signature*> & getSignatures() const {return _signatures;}

-private:
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
 			const Image & image,
@@ -162,34 +191,53 @@ protected:
 private:
 	// parameters
 	float _similarityThreshold;
-	bool _rehearsalOnlyWithLast;
 	bool _rawDataKept;
 	bool _keepRehearsedNodesInDb;
 	bool _incrementalMemory;
 	int _maxStMemSize;
 	float _recentWmRatio;
-	bool _oldDataKeptOnRehearsal;
 	bool _idUpdatedToNewOneRehearsal;
+	bool _generateIds;

 	int _idCount;
+	int _idMapCount;
 	Signature * _lastSignature;
 	int _lastLoopClosureId;
 	bool _memoryChanged; // False by default, become true when Memory::update() is called.
 	int _signaturesAdded;
-	std::vector<std::pair<int, int> > _savedLoopClosureInfo; // size 3 or 0

 	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::map<int, std::map<int, Transform> > _mapTransforms; // Transform between maps <fromMapId, <toMapId, transform> >

-	//Heypoint stuff
+	//Keypoint stuff
 	VWDictionary * _vwd;
 	KeypointDetector * _keypointDetector;
 	KeypointDescriptor * _keypointDescriptor;
-	bool _reactivatedWordsComparedToNewWords;
 	float _badSignRatio;;
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
+	int _wordsPerImageTarget; // <0=none, 0=inf
+	std::vector<float> _roiRatios; // size 4
+
+	// RGBD-SLAM stuff
+	int _icpType;
+	int _bowMinInliers;
+	float _bowInlierDistance;
+	int _bowIterations;
+	float _bowMaxDepth;
+	int _icpDecimation;
+	float _icpMaxDepth;
+	float _icpVoxelSize;
+	int _icpSamples;
+	float _icpMaxCorrespondenceDistance;
+	int _icpMaxIterations;
+	float _icpMaxFitness;
+	float _icp2MaxCorrespondenceDistance;
+	int _icp2MaxIterations;
+	float _icp2MaxFitness;
+	float _icp2CorrespondenceRatio;
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/Odometry.h
+++ b/corelib/include/rtabmap/core/Odometry.h
@@ -0,0 +1,203 @@
+/*
+ * Odometry.h
+ *
+ *  Created on: 2013-08-23
+ *      Author: Mathieu
+ */
+
+#ifndef ODOMETRY_H_
+#define ODOMETRY_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+
+#include <rtabmap/core/Parameters.h>
+
+#include <rtabmap/core/Image.h>
+
+#include <opencv2/opencv.hpp>
+
+#include <pcl/common/eigen.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+class UTimer;
+
+namespace rtabmap {
+
+class RTABMAP_EXP Odometry
+{
+public:
+	virtual ~Odometry() {}
+	Transform process(Image & image);
+	virtual void reset();
+
+	bool isLargeEnoughTransform(const Transform & transform);
+
+	//getters
+	int getMaxFeatures() const  {return _maxFeatures;}
+	int getMinInliers() const {return _minInliers;}
+	float getInlierDistance() const {return _inlierDistance;}
+	int getIterations() const {return _iterations;}
+	float getMaxDepth() const {return _maxDepth;}
+	float geLinearUpdate() const {return _linearUpdate;}
+	float getAngularUpdate() const {return _angularUpdate;}
+
+private:
+	virtual Transform computeTransform(Image & image) = 0;
+
+private:
+	int _maxFeatures;
+	int _minInliers;
+	float _inlierDistance;
+	int _iterations;
+	float _maxDepth;
+	float _linearUpdate;
+	float _angularUpdate;
+	int _resetCountdown;
+	Transform _pose;
+	int _resetCurrentCount;
+
+protected:
+	Odometry(float inlierDistance = Parameters::defaultOdomInlierDistance(),
+			int maxWords = Parameters::defaultOdomMaxWords(),
+			int minInliers = Parameters::defaultOdomMinInliers(),
+			int iterations = Parameters::defaultOdomIterations(),
+			float maxDepth = Parameters::defaultOdomMaxDepth(),
+			float linearUpdate = Parameters::defaultOdomLinearUpdate(),
+			float angularUpdate = Parameters::defaultOdomAngularUpdate(),
+			int resetCountDown = Parameters::defaultOdomResetCountdown());
+	Odometry(const rtabmap::ParametersMap & parameters);
+};
+
+class RTABMAP_EXP OdometryBinary : public Odometry
+{
+public:
+	OdometryBinary(
+			float inlierDistance = Parameters::defaultOdomInlierDistance(),
+			int maxWords = Parameters::defaultOdomMaxWords(),
+			int minInliers = Parameters::defaultOdomMinInliers(),
+			int iterations = Parameters::defaultOdomIterations(),
+			float maxDepth = Parameters::defaultOdomMaxDepth(),
+			float linearUpdate = Parameters::defaultOdomLinearUpdate(),
+			float angularUpdate = Parameters::defaultOdomAngularUpdate(),
+			int resetCountdown = Parameters::defaultOdomResetCountdown(),
+			int briefBytes = Parameters::defaultOdomBinBriefBytes(),
+			int fastThreshold = Parameters::defaultOdomBinFastThreshold(),
+			bool fastNonmaxSuppression = Parameters::defaultOdomBinFastNonmaxSuppression(),
+			bool bruteForceMatching = Parameters::defaultOdomBinBruteForceMatching());
+	OdometryBinary(const rtabmap::ParametersMap & parameters);
+	virtual ~OdometryBinary() {}
+	virtual void reset();
+
+private:
+	virtual Transform computeTransform(Image & image);
+
+private:
+	int _briefBytes;
+	int _fastThreshold;
+	bool _fastNonmaxSuppression;
+	bool _bruteForceMatching;
+
+	std::vector<cv::KeyPoint> _lastKeypoints;
+	cv::Mat _lastDescriptors;
+	cv::Mat _lastDepth;
+};
+
+class Memory;
+
+class RTABMAP_EXP OdometryBOW : public Odometry
+{
+public:
+	OdometryBOW(
+			int detectorType = Parameters::defaultKpDetectorStrategy(), // 0=SURF or 1=SIFT
+			float inlierDistance = Parameters::defaultOdomInlierDistance(),
+			int maxWords = Parameters::defaultOdomMaxWords(),
+			int minInliers = Parameters::defaultOdomMinInliers(),
+			int iterations = Parameters::defaultOdomIterations(),
+			float maxDepth = Parameters::defaultOdomMaxDepth(),
+			float linearUpdate = Parameters::defaultOdomLinearUpdate(),
+			float angularUpdate = Parameters::defaultOdomAngularUpdate(),
+			int resetCoutdown = Parameters::defaultOdomResetCountdown(),
+			float surfHessianThreshold = Parameters::defaultSURFHessianThreshold(),
+			float nndr = Parameters::defaultKpNndrRatio()); // nearest neighbor distance ratio
+	OdometryBOW(const rtabmap::ParametersMap & parameters);
+	virtual ~OdometryBOW();
+
+	virtual void reset();
+
+private:
+	virtual Transform computeTransform(Image & image);
+
+private:
+	Memory * _memory;
+};
+
+class RTABMAP_EXP OdometryICP : public Odometry
+{
+public:
+	OdometryICP(
+			int decimation = Parameters::defaultOdomICPDecimation(),
+			float voxelSize = Parameters::defaultOdomICPVoxelSize(),
+			float samples = Parameters::defaultOdomICPSamples(),
+			float maxCorrespondenceDistance = Parameters::defaultOdomICPCorrespondencesDistance(),
+			int	maxIterations = Parameters::defaultOdomICPIterations(),
+			float maxFitness = Parameters::defaultOdomICPMaxFitness(),
+			float maxDepth = Parameters::defaultOdomMaxDepth(),
+			float linearUpdate = Parameters::defaultOdomLinearUpdate(),
+			float angularUpdate = Parameters::defaultOdomAngularUpdate(),
+			int resetCoutdown = Parameters::defaultOdomResetCountdown());
+	OdometryICP(const ParametersMap & parameters);
+	void reset();
+
+private:
+	virtual Transform computeTransform(Image & image);
+
+private:
+	int _decimation;
+	float _voxelSize;
+	float _samples;
+	float _maxCorrespondenceDistance;
+	int	_maxIterations;
+	float _maxFitness;
+
+	pcl::PointCloud<pcl::PointNormal>::Ptr _previousCloud;
+};
+
+// return true if odometry is correctly computed
+Transform computeTransform(Image & image);
+
+class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
+public:
+	// take ownership of Odometry
+	OdometryThread(Odometry * odometry);
+	virtual ~OdometryThread();
+
+protected:
+	virtual void handleEvent(UEvent * event);
+
+private:
+	void mainLoopKill();
+
+	//============================================================
+	// MAIN LOOP
+	//============================================================
+	void mainLoop();
+	void addImage(const Image & image);
+	void getImage(Image & image);
+
+private:
+	USemaphore _imageAdded;
+	UMutex _imageMutex;
+	Image _imageBuffer;
+	Odometry * _odometry;
+	bool _resetOdometry;
+};
+
+} /* namespace rtabmap */
+#endif /* ODOMETRY_H_ */
--- a/corelib/include/rtabmap/core/OdometryEvent.h
+++ b/corelib/include/rtabmap/core/OdometryEvent.h
@@ -0,0 +1,43 @@
+/*
+ * OdometryEvent.h
+ *
+ *  Created on: 2013-10-15
+ *      Author: Mathieu
+ */
+
+#ifndef ODOMETRYEVENT_H_
+#define ODOMETRYEVENT_H_
+
+#include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/core/Image.h"
+
+namespace rtabmap {
+
+class OdometryEvent : public UEvent
+{
+public:
+	OdometryEvent(
+		const Image & data) :
+			_data(data) {}
+	virtual ~OdometryEvent() {}
+	virtual std::string getClassName() const {return "OdometryEvent";}
+
+	bool isValid() const {return !_data.pose().isNull();}
+	const Image & data() const {return _data;}
+
+private:
+	Image _data;
+};
+
+class OdometryResetEvent : public UEvent
+{
+public:
+	OdometryResetEvent(){}
+	virtual ~OdometryResetEvent() {}
+	virtual std::string getClassName() const {return "OdometryResetEvent";}
+};
+
+}
+
+
+#endif /* ODOMETRYEVENT_H_ */
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -29,7 +29,7 @@ namespace rtabmap
 {

 typedef std::map<std::string, std::string> ParametersMap; // Key, value
-typedef std::pair<const std::string, std::string> ParametersPair;
+typedef std::pair<std::string, std::string> ParametersPair;

 /**
 * Macro used to create parameter's key and default value.
@@ -49,14 +49,15 @@ typedef std::pair<const std::string, std::string> ParametersPair;
 * 			DummyVideoImageWidth dummyVideoImageWidth;
 * @endcode
 */
-#define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE) \
+#define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static TYPE default##PREFIX##NAME() {return DEFAULT_VALUE;} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
-			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE));} \
+			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE)); \
+								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
 // end define PARAM
@@ -80,14 +81,15 @@ typedef std::pair<const std::string, std::string> ParametersPair;
 * 			DummyVideoFileName dummyVideoFileName;
 * @endcode
 */
-#define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE) \
+#define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE, DESCRIPTION) \
 	public: \
 		static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
 		static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
 	private: \
 		class Dummy##PREFIX##NAME { \
 		public: \
-			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE));} \
+			Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE)); \
+								   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
 		}; \
 		Dummy##PREFIX##NAME dummy##PREFIX##NAME;
 // end define PARAM
@@ -119,85 +121,140 @@ typedef std::pair<const std::string, std::string> ParametersPair;
 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, PublishImage, 	             bool, true); // Publishing image
-	RTABMAP_PARAM(Rtabmap, PublishPdf, 	                 bool, true); // Publishing pdf
-	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	         bool, true); // Publishing likelihood
-	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, ImageBufferSize,              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, LikelihoodNullValuesIgnored,  bool, true); // Ignore null values on likelihood normalization
-	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true); // Statistic logs buffered in RAM instead of written to hard drive after each iteration.
-	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, true); // Logging enabled
+	RTABMAP_PARAM(Rtabmap, VhStrategy, 	                 int, 0,     "None 0, Similarity 1, Epipolar 2.");
+	RTABMAP_PARAM(Rtabmap, PublishStats, 	             bool, true, "Publishing statistics.");
+	RTABMAP_PARAM(Rtabmap, PublishImage, 	             bool, true, "Publishing image.");
+	RTABMAP_PARAM(Rtabmap, PublishPdf, 	                 bool, true, "Publishing pdf.");
+	RTABMAP_PARAM(Rtabmap, PublishLikelihood, 	         bool, true, "Publishing likelihood.");
+	RTABMAP_PARAM(Rtabmap, TimeThr, 		             float, 0.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, DetectionRate,                float, 1.0, "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
+	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              int, 1, 	 "Data buffer size (0 min inf).");
+	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory(), "Working directory.");
+	RTABMAP_PARAM_STR(Rtabmap, DatabasePath,     Parameters::getDefaultDatabasePath(), "Database path.");
+	RTABMAP_PARAM(Rtabmap, MaxRetrieved,                 unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
+	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
+	RTABMAP_PARAM(Rtabmap, StatisticLogged,   	         bool, false, "Logging enabled.");

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

 	// Memory
-	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 0.2); // Rehearsal mean for each sensor
-	RTABMAP_PARAM(Mem, RehearsalOnlyWithLast,   bool, true); // Only compare to the last signature in STM, otherwise it compares to all signatures in STM
-	RTABMAP_PARAM(Mem, ImageKept, 		        bool, true); // Keep images in db
-	RTABMAP_PARAM(Mem, RehearsedNodesKept, 	    bool, true); // Keep rehearsed ndoes in db
-	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 30); // Short-term memory size
-	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true);
-	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2); // Ratio of locations after the last loop closure in WM that cannot be transferred
-	RTABMAP_PARAM(Mem, RehearsalOldDataKept,    bool, true); // On merge, keep old data
-	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,   bool, true); // On merge, update to new id
+	RTABMAP_PARAM(Mem, RehearsalSimilarity,     float, 1.0, 	"Rehearsal similarity.");
+	RTABMAP_PARAM(Mem, ImageKept, 		        bool, true, 	"Keep images in db.");
+	RTABMAP_PARAM(Mem, RehearsedNodesKept, 	    bool, true, 	"Keep rehearsed ndoes in db.");
+	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
+	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, othwersize it is Localization mode.");
+	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
+	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
+	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location Ids, False=use input image ids.")

 	// KeypointMemory (Keypoint-based)
-	RTABMAP_PARAM(Kp, PublishKeypoints,      bool, true); // Publishing keypoints
-	RTABMAP_PARAM(Kp, NNStrategy,                int, 1);     // Naive 0, kdForest 1
-	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true);
-	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400);
-	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.2); //Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)
-	RTABMAP_PARAM(Kp, MinDistUsed,           bool, false); // The nearest neighbor must have a distance < minDist
-	RTABMAP_PARAM(Kp, MinDist, 	             float, 0.05); // Matching a descriptor with a word (euclidean distance ^ 2)
-	RTABMAP_PARAM(Kp, NndrUsed, 	         bool, true);  // If NNDR ratio is used
-	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8);  // NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)
-	RTABMAP_PARAM(Kp, MaxLeafs, 	         int, 64);  // Maximum number of leafs checked (when using kd-trees)
-	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0);      // Surf detector 0, SIFT detector 1, undef 2
-	RTABMAP_PARAM(Kp, DescriptorStrategy,    int, 0);      // kDescriptorSurf=0, kDescriptorSift, kDescriptorUndef
-	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
-	RTABMAP_PARAM_STR(Kp, RoiRatios,         "0.0 0.0 0.0 0.0"); // Region of interest ratios [left, right, top, bottom]
-	RTABMAP_PARAM_STR(Kp, DictionaryPath,    ""); // Path of the pre-computed dictionary
+	RTABMAP_PARAM(Kp, PublishKeypoints,      bool, true, 		"Publishing keypoints.");
+	RTABMAP_PARAM(Kp, NNStrategy,            int, 1, 	 		"Naive 0, kdForest 1.");
+	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
+	RTABMAP_PARAM(Kp, WordsPerImage,         int, 400, 			"");
+	RTABMAP_PARAM(Kp, BadSignRatio,          float, 0.2, 		"Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
+	RTABMAP_PARAM(Kp, MinDistUsed,           bool, false, 		"The nearest neighbor must have a distance < minDist.");
+	RTABMAP_PARAM(Kp, MinDist, 	             float, 0.05, 		"Matching a descriptor with a word (euclidean distance ^ 2)");
+	RTABMAP_PARAM(Kp, NndrUsed, 	         bool, true, 		"If NNDR ratio is used.");
+	RTABMAP_PARAM(Kp, NndrRatio, 	         float, 0.8, 		"NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
+	RTABMAP_PARAM(Kp, MaxLeafs, 	         int, 64, 			"Maximum number of leafs checked (when using kd-trees).");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0, 			"Surf detector 0, SIFT detector 1, undef 2.");
+	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.");
+	RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", 		"Region of interest ratios [left, right, top, bottom].");
+	RTABMAP_PARAM_STR(Kp, DictionaryPath,    "", 				"Path of the pre-computed dictionary");

 	//Database
-	RTABMAP_PARAM(Db, ImagesCompressed,    bool, true);	 	 // Images are compressed when saving to database
-	RTABMAP_PARAM(DbSqlite3, InMemory, 	   bool, false);	 // Using database in the memory instead of a file on the hard disk
-	RTABMAP_PARAM(DbSqlite3, CacheSize,    unsigned int, 10000);	 // Sqlite cache size (default is 2000)
-	RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3);	 	 // 0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : "PRAGMA journal_mode")
-	RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0);	 	 // 0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : "PRAGMA synchronous")
-	RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2);	 	 // 0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : "PRAGMA temp_store")
+	RTABMAP_PARAM(DbSqlite3, InMemory, 	   bool, true, 			"Using database in the memory instead of a file on the hard disk.");
+	RTABMAP_PARAM(DbSqlite3, CacheSize,    unsigned int, 10000, "Sqlite cache size (default is 2000).");
+	RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3, 				"0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")");
+	RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0, 				"0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")");
+	RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2, 				"0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");

 	// Keypoints descriptors/detectors
-	RTABMAP_PARAM(SURF, Extended, 		  bool, false); // true=128, false=64
-	RTABMAP_PARAM(SURF, HessianThreshold, float, 150.0);
-	RTABMAP_PARAM(SURF, Octaves, 		  int, 4);
-	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2);
-	RTABMAP_PARAM(SURF, Upright, 	      bool, false); // U-SURF
-	RTABMAP_PARAM(SURF, GpuVersion, 	  bool, false);
+	RTABMAP_PARAM(SURF, Extended, 		  bool, false, 	"true=128, false=64.");
+	RTABMAP_PARAM(SURF, HessianThreshold, float, 150.0,	"");
+	RTABMAP_PARAM(SURF, Octaves, 		  int, 4, 		"");
+	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2, 		"");
+	RTABMAP_PARAM(SURF, Upright, 	      bool, false, 	"U-SURF");
+	RTABMAP_PARAM(SURF, GpuVersion, 	  bool, false, 	"");

-	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(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, "");

 	// BayesFilter
-	RTABMAP_PARAM(Bayes, VirtualPlacePriorThr,           float, 0.9);	// Virtual place prior
-	RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23"); // Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}
-	RTABMAP_PARAM(Bayes, FullPredictionUpdate,           bool, true); // Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).
+	RTABMAP_PARAM(Bayes, VirtualPlacePriorThr,           float, 0.9, "Virtual place prior");
+	RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
+	RTABMAP_PARAM(Bayes, FullPredictionUpdate,           bool, true, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");

 	// Verify hypotheses
-	RTABMAP_PARAM(VhEp, MatchCountMin, int, 8);     // Minimum of matching visual words pairs to accept the loop hypothesis
-	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3.0);  // Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier
-	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99); // Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC
+	RTABMAP_PARAM(VhEp, MatchCountMin, int, 8, 		"Minimum of matching visual words pairs to accept the loop hypothesis.");
+	RTABMAP_PARAM(VhEp, RansacParam1,  float, 3.0, 	"Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
+	RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC.");
+
+	// RGB-D SLAM
+	RTABMAP_PARAM(RGBD, Enabled,           bool, true, 	"");
+	RTABMAP_PARAM(RGBD, ScanMatchingSize,  int, 0, 		"Laser scan matching history for odometry correction (laser scans are required). Set to 0 to disable odometry correction.");
+	RTABMAP_PARAM(RGBD, LinearUpdate,      float, 0.0, 	"Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+	RTABMAP_PARAM(RGBD, AngularUpdate,     float, 0.0, 	"Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
+
+	// Local loop closure detection
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, true, 	"Detection over all locations in STM.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius,   float, 15, 		"Maximum radius for space detection.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors,   int, 20, 	"Maximum nearest neighbor.");
+
+	// Odometry
+	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=BOW 1=Binary.");
+	RTABMAP_PARAM(Odom, LinearUpdate,           float, 0.0, 	"Min linear displacement to update odometry.");
+	RTABMAP_PARAM(Odom, AngularUpdate,          float, 0.0, 	"Min angular displacement to update odometry.");
+	RTABMAP_PARAM(Odom, MaxWords,               int, 0, 		"0 no limits.");
+	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.005, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
+	RTABMAP_PARAM(Odom, Iterations,             int, 100, 		"Maximum iterations to compute the transform from visual words.");
+	RTABMAP_PARAM(Odom, MaxDepth,               float, 5.0, 	"Max depth of the words (0 means no limit).");
+	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).")
+
+	RTABMAP_PARAM(OdomBin, BriefBytes,         	   int, 32, 	"");
+	RTABMAP_PARAM(OdomBin, FastThreshold,          int, 30, 	"");
+	RTABMAP_PARAM(OdomBin, FastNonmaxSuppression,  bool, true, 	"");
+	RTABMAP_PARAM(OdomBin, BruteForceMatching,     bool, true, 	"If false, FLANN LSH is used.");
+
+	RTABMAP_PARAM(OdomICP, Decimation,               int, 4, 		"");
+	RTABMAP_PARAM(OdomICP, VoxelSize,                float, 0.005, 	"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(OdomICP, Samples,                  int, 0, 		"not used if voxelSize is set.");
+	RTABMAP_PARAM(OdomICP, CorrespondencesDistance,  float, 0.05, 	"");
+	RTABMAP_PARAM(OdomICP, Iterations,               int, 30, 		"");
+	RTABMAP_PARAM(OdomICP, MaxFitness,               float, 0.01, 	"");
+
+	// Loop closure constraint
+	RTABMAP_PARAM(LccIcp, Enabled,         bool, false, 	"Enable ICP");
+	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=ICP 3D, 1=ICP 2D");
+
+	RTABMAP_PARAM(LccBow, MinInliers,      int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
+	RTABMAP_PARAM(LccBow, InlierDistance,  float, 0.01, 	"Maximum distance for visual word correspondences.");
+	RTABMAP_PARAM(LccBow, Iterations,      int, 100, 		"Maximum iterations to compute the transform from visual words.");
+	RTABMAP_PARAM(LccBow, MaxDepth,        float, 5.0, 		"Max depth of the words (0 means no limit).");
+
+	RTABMAP_PARAM(LccIcp3, Decimation,      int, 8, 		"Depth image decimation.");
+	RTABMAP_PARAM(LccIcp3, MaxDepth,        float, 4.0, 	"Max cloud depth.");
+	RTABMAP_PARAM(LccIcp3, VoxelSize,       float, 0.005, 	"Voxel size to be used for ICP computation.");
+	RTABMAP_PARAM(LccIcp3, Samples,         int, 0, 		"Random samples to be used for ICP computation. Not used if voxelSize is set.");
+	RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
+	RTABMAP_PARAM(LccIcp3, Iterations,      int, 30, 		"ICP 3D: Max iterations.");
+	RTABMAP_PARAM(LccIcp3, MaxFitness,      float, 1.0, 	"ICP 3D: Maximum fitness to accept the computed transform.");
+
+	RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, 	"ICP 2D: Max distance for point correspondences.");
+	RTABMAP_PARAM(LccIcp2, Iterations,      int, 30, 				"ICP 2D: Max iterations.");
+	RTABMAP_PARAM(LccIcp2, MaxFitness,      float, 1.0, 			"ICP 2D: Maximum fitness to accept the computed transform.");
+	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"ICP 2D: Ratio of matching correspondences to accept the transform.");
+

 public:
 	virtual ~Parameters();
@@ -211,12 +268,29 @@ public:
 		return parameters_;
 	}

+	/**
+	 * Get parameter description
+	 *
+	 */
+	static std::string getDescription(const std::string & paramKey);
+
+	static void parse(const ParametersMap & parameters, const std::string & key, bool & value);
+	static void parse(const ParametersMap & parameters, const std::string & key, int & value);
+	static void parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
+	static void parse(const ParametersMap & parameters, const std::string & key, float & value);
+	static void parse(const ParametersMap & parameters, const std::string & key, double & value);
+	static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
+
+	static std::string getDefaultDatabaseName();
+
 private:
 	Parameters();
 	static std::string getDefaultWorkingDirectory();
+	static std::string getDefaultDatabasePath();

 private:
 	static ParametersMap parameters_;
+	static ParametersMap descriptions_;
 	static Parameters instance_;
 };

--- a/corelib/include/rtabmap/core/Rtabmap.h
+++ b/corelib/include/rtabmap/core/Rtabmap.h
@@ -37,12 +37,12 @@ namespace rtabmap
 class EpipolarGeometry;
 class Memory;
 class BayesFilter;
+class Signature;

 class RTABMAP_EXP Rtabmap
 {
 public:
 	enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
-	static const char * kDefaultDatabaseName;

 public:
 	static std::string getVersion();
@@ -53,8 +53,8 @@ public:
 	Rtabmap();
 	virtual ~Rtabmap();

-	void process(const cv::Mat & image, int id=0, std::multimap<int, cv::KeyPoint> * words = 0); // for convenience, an id is automatically generated if id=0
-	void process(const Image & image, std::multimap<int, cv::KeyPoint> * words = 0); // for convenience
+	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
+	bool process(const Image & image); // for convenience

 	void init(const ParametersMap & param, bool deleteMemory = true);
 	void init(const std::string & configFile = "", bool deleteMemory = true);
@@ -62,6 +62,7 @@ public:
 	void close();

 	const std::string & getWorkingDir() const {return _wDir;}
+	std::string getDatabasePath() const;
 	int getLoopClosureId() const;
 	int getRetrievedId() const;
 	int getLastLocationId() const;
@@ -76,25 +77,39 @@ public:
 	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
 	std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;// <Id,Margin> including nodeId
 	bool isInSTM(int locationId) const;
+	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
+	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
+	Transform getPose(int locationId) const;
+	Transform getMapCorrection() const {return _mapCorrection;}

 	void setTimeThreshold(float maxTimeAllowed); // in ms

+	void triggerNewMap();
 	void generateGraph(const std::string & path, int id=0, int margin=5);
 	void resetMemory(bool dbOverwritten = false);
 	void dumpPrediction() const;
 	void dumpData() const;
 	void parseParameters(const ParametersMap & parameters);
 	void setWorkingDirectory(std::string path);
-	void deleteLastLocation();
+	void setDatabasePath(const std::string & path);
 	void deleteLocation(int locationId); // Only nodes in STM can be deleted
-	void rejectLastLoopClosure();
+	void rejectLoopClosure(int oldId, int newId);
+	void get3DMap(std::map<int, std::vector<unsigned char> > & images,
+			std::map<int, std::vector<unsigned char> > & depths,
+			std::map<int, std::vector<unsigned char> > & depths2d,
+			std::map<int, float> & depthConstants,
+			std::map<int, Transform> & localTransforms,
+			std::map<int, Transform> & poses,
+			Transform & mapCorrection) const;

+	std::map<int, Transform> getOptimizedWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int & nearestId) const;
 	void adjustLikelihood(std::map<int, float> & likelihood) const;
 	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
 											const std::map<int, float> & likelihood) const;

 private:
+	void optimizeCurrentMap(int id, bool lookInDatabase, std::map<int, Transform> & optimizedPoses, Transform & mapCorrection) const;
 	void setupLogFiles(bool overwrite = false);
 	void flushStatisticLogs();

@@ -110,9 +125,18 @@ private:
 	float _loopThr;
 	float _loopRatio;
 	unsigned int _maxRetrieved;
-	bool _likelihoodNullValuesIgnored;
 	bool _statisticLogsBufferedInRAM;
 	bool _statisticLogged;
+	bool _rgbdSlamMode;
+	float _rgbdLinearUpdate;
+	float _rgbdAngularUpdate;
+	int _scanMatchingSize;
+	bool _localLoopClosureDetectionTime;
+	bool _localLoopClosureDetectionSpace;
+	float _localDetectRadius;
+	float _localDetectMaxNeighbors;
+	bool _icpEnabled;
+	std::string _databasePath;

 	int _lcHypothesisId;
 	float _lcHypothesisValue;
@@ -134,6 +158,9 @@ private:
 	Statistics statistics_;

 	std::string _wDir;
+
+	std::map<int, Transform> _optimizedPoses;
+	Transform _mapCorrection;
 };

 #endif /* RTABMAP_H_ */
--- a/corelib/include/rtabmap/core/RtabmapEvent.h
+++ b/corelib/include/rtabmap/core/RtabmapEvent.h
@@ -55,21 +55,42 @@ public:
 			kCmdGenerateGraph,
 			kCmdGenerateLocalGraph,
 			kCmdDeleteMemory,
-			kCmdCleanSensorsBuffer};
+			kCmdCleanDataBuffer,
+			kCmdPublish3DMap,
+			kCmdTriggerNewMap,
+			kCmdPause};
 public:
 	RtabmapEventCmd(Cmd cmd) :
 		UEvent(0),
-		_cmd(cmd) {}
+		_cmd(cmd),
+		_strValue(""),
+		_intValue(0){}
+	RtabmapEventCmd(Cmd cmd, int value) :
+			UEvent(0),
+			_cmd(cmd),
+			_strValue(""),
+			_intValue(value){}
+	RtabmapEventCmd(Cmd cmd, const std::string & value) :
+			UEvent(0),
+			_cmd(cmd),
+			_strValue(value),
+			_intValue(0){}

 	virtual ~RtabmapEventCmd() {}
 	Cmd getCmd() const {return _cmd;}
-	void setStr(const std::string & str) {_str = str;}
-	const std::string & getStr() const {return _str;}
+
+	void setStr(const std::string & str) {_strValue = str;}
+	const std::string & getStr() const {return _strValue;}
+
+	void setInt(int v) {_intValue = v;}
+	int getInt() const {return _intValue;}
+
 	virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}

 private:
 	Cmd _cmd;
-	std::string _str;
+	std::string _strValue;
+	int _intValue;
 };

 class RtabmapEventInit : public UEvent
@@ -107,6 +128,51 @@ private:
 	std::string _info; // "Loading signatures", "Loading words" ...
 };

+class RtabmapEvent3DMap : public UEvent
+{
+public:
+	RtabmapEvent3DMap(int codeError = 0):
+		UEvent(codeError){}
+	RtabmapEvent3DMap(
+			const std::map<int, std::vector<unsigned char> > & images,
+			const std::map<int, std::vector<unsigned char> > & depths,
+			const std::map<int, std::vector<unsigned char> > & depths2d,
+			const std::map<int, float> & depthConstants,
+			const std::map<int, Transform> & localTransforms,
+			const std::map<int, Transform> & poses,
+			const Transform & mapCorrection) :
+		UEvent(0),
+		_images(images),
+		_depths(depths),
+		_depths2d(depths2d),
+		_depthConstants(depthConstants),
+		_localTransforms(localTransforms),
+		_poses(poses),
+		_mapCorrection(mapCorrection)
+	{}
+
+	virtual ~RtabmapEvent3DMap() {}
+
+	const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
+	const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
+	const std::map<int, std::vector<unsigned char> > & getDepths2d() const {return _depths2d;}
+	const std::map<int, float> & getDepthConstants() const {return _depthConstants;}
+	const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
+	const std::map<int, Transform> & getPoses() const {return _poses;}
+	const Transform & getMapCorrection() const {return _mapCorrection;}
+
+	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
+
+private:
+	std::map<int, std::vector<unsigned char> > _images;
+	std::map<int, std::vector<unsigned char> > _depths;
+	std::map<int, std::vector<unsigned char> > _depths2d;
+	std::map<int, float> _depthConstants;
+	std::map<int, Transform> _localTransforms;
+	std::map<int, Transform> _poses;
+	Transform _mapCorrection;
+};
+
 } // namespace rtabmap

 #endif /* RTABMAPEVENT_H_ */
--- a/corelib/include/rtabmap/core/RtabmapExp.h
+++ b/corelib/include/rtabmap/core/RtabmapExp.h
@@ -21,7 +21,7 @@
 #define RTABMAPEXP_H

 #if defined(_WIN32)
-  #if defined(rtabmap_corelib_EXPORTS) || defined(rtabmap_guilib_EXPORTS)
+  #if defined(rtabmap_core_EXPORTS)
    #define RTABMAP_EXP   __declspec( dllexport )
  #else
    #define RTABMAP_EXP   __declspec( dllimport )
--- a/corelib/include/rtabmap/core/RtabmapThread.h
+++ b/corelib/include/rtabmap/core/RtabmapThread.h
@@ -33,6 +33,8 @@

 #include <stack>

+class UTimer;
+
 namespace rtabmap {

 class Rtabmap;
@@ -52,15 +54,16 @@ public:
 		kStateGeneratingGraph,
 		kStateGeneratingLocalGraph,
 		kStateDeletingMemory,
-		kStateCleanSensorsBuffer
+		kStateCleanDataBuffer,
+		kStatePublishingMap,
+		kStateTriggeringMap
 	};

 public:
 	RtabmapThread();
 	virtual ~RtabmapThread();

-	void setWorkingDirectory(const std::string & path);
-	void clearBufferedSensors();
+	void clearBufferedData();

 protected:
 	virtual void handleEvent(UEvent * anEvent);
@@ -73,6 +76,7 @@ private:
 	void getImage(Image & image);
 	void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
 	void setDataBufferSize(int size);
+	void publishMap() const;

 private:
 	UMutex _stateMutex;
@@ -83,8 +87,11 @@ private:
 	UMutex _imageMutex;
 	USemaphore _imageAdded;
 	int _imageBufferMaxSize;
+	float _rate;
+	UTimer * _frameRateTimer;

 	Rtabmap * _rtabmap;
+	bool _paused;
 };

 } /* namespace rtabmap */
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -0,0 +1,147 @@
+/*
+ * 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 <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <map>
+#include <list>
+#include <vector>
+#include <set>
+
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap
+{
+
+class Memory;
+
+class RTABMAP_EXP Signature
+{
+
+public:
+	Signature(int id,
+			int mapId,
+			const std::multimap<int, cv::KeyPoint> & words,
+			const std::multimap<int, pcl::PointXYZ> & words3,
+			const Transform & pose = Transform(),
+			const std::vector<unsigned char> & depth2D = std::vector<unsigned char>(),
+			const std::vector<unsigned char> & image = std::vector<unsigned char>(),
+			const std::vector<unsigned char> & depth = std::vector<unsigned char>(),
+			float depthConstant = 0.0f,
+			const Transform & localTransform =Transform::getIdentity());
+	virtual ~Signature();
+
+	/**
+	 * Must return a value between >=0 and <=1 (1 means 100% similarity).
+	 */
+	float compareTo(const Signature * signature) const;
+	bool isBadSignature() const;
+
+	int id() const {return _id;}
+	int mapId() const {return _mapId;}
+
+	void addNeighbors(const std::map<int, Transform> & neighbors);
+	void addNeighbor(int neighbor, const Transform & transform = Transform());
+	void removeNeighbor(int neighborId);
+	void removeNeighbors();
+	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
+	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
+
+	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
+	void setLoopClosureIds(const std::map<int, Transform> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
+	void addLoopClosureId(int loopClosureId, const Transform & transform = Transform());
+	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
+	void changeLoopClosureId(int idFrom, int idTo);
+
+	void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
+	void setChildLoopClosureIds(const std::map<int, Transform> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
+	void addChildLoopClosureId(int childLoopClosureId, const Transform & transform = Transform());
+
+	void setSaved(bool saved) {_saved = saved;}
+	void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
+	void changeNeighborIds(int idFrom, int idTo);
+
+	const std::map<int, Transform> & getNeighbors() const {return _neighbors;}
+	int getWeight() const {return _weight;}
+	const std::map<int, Transform> & getLoopClosureIds() const {return _loopClosureIds;}
+	const std::map<int, Transform> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
+	bool isSaved() const {return _saved;}
+	bool isModified() const {return _modified || _neighborsModified;}
+	bool isNeighborsModified() const {return _neighborsModified;}
+
+	//visual words stuff
+	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;}
+	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
+	void setImage(const std::vector<unsigned char> & image) {_image = image;}
+	const std::vector<unsigned char> & getImage() const {return _image;}
+
+	//metric stuff
+	void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
+	void setDepth(const std::vector<unsigned char> & depth, float depthConstant);
+	void setDepth2D(const std::vector<unsigned char> & depth2D) {_depth2D = depth2D;}
+	void setLocalTransform(const Transform & t) {_localTransform = t;}
+	void setPose(const Transform & pose) {_pose = pose;}
+	const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
+	const std::vector<unsigned char> & getDepth() const {return _depth;}
+	const std::vector<unsigned char> & getDepth2D() const {return _depth2D;}
+	float getDepthConstant() const {return _depthConstant;}
+	const Transform & getPose() const {return _pose;}
+	const Transform & getLocalTransform() const {return _localTransform;}
+
+private:
+	int _id;
+	int _mapId;
+	std::map<int, Transform> _neighbors; // id, transform
+	int _weight;
+	std::map<int, Transform> _loopClosureIds; // id, transform
+	std::map<int, Transform> _childLoopClosureIds; // id, transform
+	bool _saved; // If it's saved to bd
+	bool _modified;
+	bool _neighborsModified; // Optimization when updating signatures in database
+
+	// 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>
+	std::map<int, int> _wordsChanged; // <oldId, newId>
+	bool _enabled;
+	std::vector<unsigned char> _image; //compressed image CV_8UC1 or CV_8UC3
+
+	std::vector<unsigned char> _depth; // compressed image CV_16UC1
+	std::vector<unsigned char> _depth2D; // compressed data CV_32FC2
+	float _depthConstant;
+	Transform _pose;
+	Transform _localTransform; // camera_link -> base_link
+	std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
+};
+
+} // namespace rtabmap
--- a/corelib/include/rtabmap/core/Statistics.h
+++ b/corelib/include/rtabmap/core/Statistics.h
@@ -27,6 +27,7 @@
 #include <opencv2/imgproc/imgproc.hpp>
 #include <list>
 #include <vector>
+#include <rtabmap/core/Transform.h>

 namespace rtabmap {

@@ -48,17 +49,30 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Vp_hypothesis,);
 	RTABMAP_STATS(Loop, ReactivateId,);
 	RTABMAP_STATS(Loop, Hypothesis_ratio,);
+	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
+
+	RTABMAP_STATS(LocalLoop, Scan_matching_success,);
+	RTABMAP_STATS(LocalLoop, Time_closures,);
+	RTABMAP_STATS(LocalLoop, Space_closure_id,);
+	RTABMAP_STATS(LocalLoop, Space_neighbors,);

 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
 	RTABMAP_STATS(Memory, Signatures_removed,);
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
+	RTABMAP_STATS(Memory, Rehearsal_sim,);
+	RTABMAP_STATS(Memory, Rehearsal_merged,);
+	RTABMAP_STATS(Memory, Last_loop_closure,);

 	RTABMAP_STATS(Timing, Memory_update, ms);
+	RTABMAP_STATS(Timing, Scan_matching, ms);
+	RTABMAP_STATS(Timing, Local_detection_TIME, ms);
+	RTABMAP_STATS(Timing, Local_detection_SPACE, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
 	RTABMAP_STATS(Timing, Reactivation, ms);
 	RTABMAP_STATS(Timing, Add_loop_closure_link, ms);
+	RTABMAP_STATS(Timing, Map_optimization, ms);
 	RTABMAP_STATS(Timing, Likelihood_computation, ms);
 	RTABMAP_STATS(Timing, Posterior_computation, ms);
 	RTABMAP_STATS(Timing, Hypotheses_creation, ms);
@@ -70,7 +84,9 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Timing, Joining_trash, ms);
 	RTABMAP_STATS(Timing, Emptying_trash, ms);

-	RTABMAP_STATS(, Hypothesis_reactivated,);
+	RTABMAP_STATS(TimingMem, Pre_update, ms);
+	RTABMAP_STATS(TimingMem, Signature_creation, ms);
+	RTABMAP_STATS(TimingMem, Rehearsal, ms);

 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
 	RTABMAP_STATS(Keypoint, Response_threshold,);
@@ -88,10 +104,25 @@ public:
 	// setters
 	void setExtended(bool extended) {_extended = extended;}
 	void setRefImageId(int refImageId) {_refImageId = refImageId;}
+	void setRefImageMapId(int refImageMapId) {_refImageMapId = refImageMapId;}
 	void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
+	void setLoopClosureMapId(int loopClosureMapId) {_loopClosureMapId = loopClosureMapId;}
 	void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
-	void setRefImage(const cv::Mat & image);
-	void setLoopImage(const cv::Mat & image);
+	void setLocalLoopClosureMapId(int localLoopClosureMapId) {_localLoopClosureMapId = localLoopClosureMapId;}
+	void setRefImage(const std::vector<unsigned char> & image) {_refImage = image;}
+	void setLoopImage(const std::vector<unsigned char> & image) {_loopImage = image;}
+	void setRefDepth(const std::vector<unsigned char> & depth) {_refDepth = depth;}
+	void setRefDepth2D(const std::vector<unsigned char> & depth2d) {_refDepth2d = depth2d;}
+	void setLoopDepth(const std::vector<unsigned char> & depth) {_loopDepth = depth;}
+	void setLoopDepth2D(const std::vector<unsigned char> & depth2d) {_loopDepth2d = depth2d;}
+	void setRefDepthConstant(float depthConstant) {_refDepthConstant = depthConstant;}
+	void setLoopDepthConstant(float depthConstant) {_loopDepthConstant = depthConstant;}
+	void setRefLocalTransform(const Transform & localTransform) {_refLocalTransform = localTransform;}
+	void setLoopLocalTransform(const Transform & localTransform) {_loopLocalTransform = localTransform;}
+	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
+	void setCurrentPose(const Transform & pose) {_currentPose = pose;}
+	void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
+	void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
 	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;}
@@ -102,10 +133,25 @@ public:
 	// getters
 	bool extended() const {return _extended;}
 	int refImageId() const {return _refImageId;}
+	int refImageMapId() const {return _refImageMapId;}
 	int loopClosureId() const {return _loopClosureId;}
+	int loopClosureMapId() const {return _loopClosureMapId;}
 	int localLoopClosureId() const {return _localLoopClosureId;}
-	const cv::Mat & refImage() const {return _refImage;}
-	const cv::Mat & loopImage() const {return _loopImage;}
+	int localLoopClosureMapId() const {return _localLoopClosureMapId;}
+	const std::vector<unsigned char> & refImage() const {return _refImage;}
+	const std::vector<unsigned char> & loopImage() const {return _loopImage;}
+	const std::vector<unsigned char> & refDepth() const {return _refDepth;}
+	const std::vector<unsigned char> & loopDepth() const {return _loopDepth;}
+	const std::vector<unsigned char> & refDepth2D() const {return _refDepth2d;}
+	const std::vector<unsigned char> & loopDepth2D() const {return _loopDepth2d;}
+	float refDepthConstant() const {return _refDepthConstant;}
+	float loopDepthConstant() const {return _loopDepthConstant;}
+	const Transform & refLocalTransform() const {return _refLocalTransform;}
+	const Transform & loopLocalTransform() const {return _loopLocalTransform;}
+	const std::map<int, Transform> & poses() const {return _poses;}
+	const Transform & currentPose() const {return _currentPose;}
+	const Transform & mapCorrection() const {return _mapCorrection;}
+	const Transform & loopClosureTransform() const {return _loopClosureTransform;}
 	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;}
@@ -116,15 +162,33 @@ public:
 	const std::map<std::string, float> & data() const {return _data;}

 private:
-	int _extended; // 0 -> only loop closure and last signature ID fields are filled
+	bool _extended; // 0 -> only loop closure and last signature ID fields are filled

 	int _refImageId;
+	int _refImageMapId;
 	int _loopClosureId;
-	int _localLoopClosureId; // Note: used by VSLAM
+	int _loopClosureMapId;
+	int _localLoopClosureId;
+	int _localLoopClosureMapId;

 	// extended data start here...
-	cv::Mat _refImage;
-	cv::Mat _loopImage;
+	std::vector<unsigned char> _refImage;
+	std::vector<unsigned char> _loopImage;
+
+	// Metric data
+	std::vector<unsigned char> _refDepth;
+	std::vector<unsigned char> _refDepth2d;
+	std::vector<unsigned char> _loopDepth;
+	std::vector<unsigned char> _loopDepth2d;
+	float _refDepthConstant;
+	float _loopDepthConstant;
+	Transform _refLocalTransform;
+	Transform _loopLocalTransform;
+
+	std::map<int, Transform> _poses;
+	Transform _currentPose;
+	Transform _mapCorrection;
+	Transform _loopClosureTransform;

 	std::map<int, int> _weights;
 	std::map<int, float> _posterior;
--- a/corelib/include/rtabmap/core/Transform.h
+++ b/corelib/include/rtabmap/core/Transform.h
@@ -0,0 +1,72 @@
+/*
+ * Transform.h
+ *
+ *  Created on: 2013-08-30
+ *      Author: Mathieu
+ */
+
+#ifndef TRANSFORM_H_
+#define TRANSFORM_H_
+
+#include <rtabmap/core/RtabmapExp.h>
+#include <vector>
+#include <string>
+
+namespace rtabmap {
+
+class RTABMAP_EXP Transform
+{
+public:
+
+	// Zero by default
+	Transform();
+	// rotation matrix r## and origin o##
+	Transform(float r11, float r12, float r13, float o14,
+		      float r21, float r22, float r23, float o24,
+			  float r31, float r32, float r33, float o34);
+	// x,y,z, roll,pitch,yaw
+	Transform(float x, float y, float z, float roll, float pitch, float yaw);
+
+	float & operator[](int index) {return data_[index];}
+	const float & operator[](int index) const {return data_[index];}
+
+	bool isNull() const;
+	bool isIdentity() const;
+
+	void setNull();
+	void setIdentity();
+
+	const float * data() const {return data_.data();}
+	float * data() {return data_.data();}
+	int size() const {return data_.size();}
+
+	float & x() {return data_[3];}
+	float & y() {return data_[7];}
+	float & z() {return data_[11];}
+	const float & x() const {return data_[3];}
+	const float & y() const {return data_[7];}
+	const float & z() const {return data_[11];}
+
+	Transform inverse() const;
+	Transform rotation() const;
+	Transform translation() const;
+
+	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
+	std::string prettyPrint() const;
+
+	Transform operator*(const Transform & t) const;
+	Transform & operator*=(const Transform & t);
+	bool operator==(const Transform & t) const;
+	bool operator!=(const Transform & t) const;
+
+	static Transform getIdentity();
+
+private:
+	std::vector<float> data_;
+};
+
+RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const Transform& s);
+
+}
+
+#endif /* TRANSFORM_H_ */
--- a/corelib/include/rtabmap/core/VWDictionary.h
+++ b/corelib/include/rtabmap/core/VWDictionary.h
@@ -25,6 +25,7 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
+#include <set>
 #include "rtabmap/core/Parameters.h"

 namespace rtabmap
@@ -64,16 +65,13 @@ public:
 	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;}
+	unsigned int getNotIndexedWordsCount() const {return _notIndexedWords.size();}
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
 	void setNNStrategy(NNStrategy strategy, const ParametersMap & parameters = ParametersMap());
@@ -93,7 +91,6 @@ protected:

 protected:
 	std::map<int, VisualWord *> _visualWords; //<id,VisualWord*>
-	unsigned int _lastNewWordsAddedCount;
 	int _totalActiveReferences; // keep track of all references for updating the common signature

 private:
@@ -110,6 +107,8 @@ private:
 	cv::Mat _dataTree;
 	std::map<int ,int> _mapIndexId;
 	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
+	std::set<int> _notIndexedWords; // Words that are not indexed in the dictionary
+	std::set<int> _removedIndexedWords; // Words not anymore in the dictionary but still indexed in the dictionary
 };

 } // namespace rtabmap
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -0,0 +1,362 @@
+/*
+ * Util3D.h
+ *      Author: mathieu
+ */
+
+#ifndef UTIL3D_H_
+#define UTIL3D_H_
+
+#include "rtabmap/core/RtabmapExp.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include <string>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UThread.h>
+#include <pcl/common/eigen.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/PolygonMesh.h>
+
+namespace rtabmap
+{
+
+class Signature;
+
+namespace util3d
+{
+
+/**
+ * Compress image or data
+ *
+ * Example compression:
+ *   cv::Mat image;// an image
+ *   CompressionThread ct(image);
+ *   ct.start();
+ *   ct.join();
+ *   std::vector<unsigned char> bytes = ct.getCompressedData();
+ *
+ * Example uncompression
+ *   std::vector<unsigned char> bytes;// a compressed image
+ *   CompressionThread ct(bytes);
+ *   ct.start();
+ *   ct.join();
+ *   cv::Mat image = ct.getUncompressedData();
+ */
+class RTABMAP_EXP CompressionThread : public UThread
+{
+public:
+	// format : ".png" ".jpg" "" (empty is general)
+	CompressionThread(const cv::Mat & mat, const std::string & format = "");
+	CompressionThread(const std::vector<unsigned char> & bytes, bool isImage);
+	const std::vector<unsigned char> & getCompressedData() const {return compressedData_;}
+	cv::Mat & getUncompressedData() {return uncompressedData_;}
+protected:
+	virtual void mainLoop();
+private:
+	std::vector<unsigned char> compressedData_;
+	cv::Mat uncompressedData_;
+	std::string format_;
+	bool image_;
+	bool compressMode_;
+};
+
+cv::Mat RTABMAP_EXP rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder = true);
+cv::Mat RTABMAP_EXP depthFromCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
+		float & fx,
+		float & fy,
+		bool depth16U = true);
+void RTABMAP_EXP rgbdFromCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
+		cv::Mat & rgb,
+		cv::Mat & depth,
+		float & fx,
+		float & fy,
+		bool bgrOrder = true,
+		bool depth16U = true);
+
+cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
+cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
+
+std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
+		const std::multimap<int, cv::KeyPoint> & words,
+		const cv::Mat & depth,
+		float depthConstant,
+		const Transform & transform);
+
+std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
+		const std::list<int> & wordIds,
+		const std::vector<cv::KeyPoint> & keypoints);
+
+pcl::PointXYZ RTABMAP_EXP getDepth(const cv::Mat & depthImage,
+					   int x, int y,
+					   float cx, float cy,
+					   float fx, float fy);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float voxelSize);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float voxelSize);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP sampling(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		int samples);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
+
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Transform & transform);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+		const cv::Mat & imageDepth,
+		float depthConstant,
+		int decimation);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
+		const cv::Mat & imageDepth,
+		float cx, float cy,
+		float fx, float fy,
+		int decimation = 1);
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDepth,
+		float depthConstant,
+		int decimation = 1);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDepth,
+		float cx, float cy,
+		float fx, float fy,
+		int decimation = 1);
+
+cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
+
+std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
+cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
+
+std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
+cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
+
+// remove depth by z axis
+void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
+									  const std::multimap<int, pcl::PointXYZ> & words2,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud1,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
+void RTABMAP_EXP extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
+									  const std::multimap<int, pcl::PointXYZ> & words2,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud1,
+									  pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
+void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
+									   const cv::Mat & depthImage1,
+									   const cv::Mat & depthImage2,
+									   float cx, float cy,
+									   float fx, float fy,
+									   float maxDepth,
+									   pcl::PointCloud<pcl::PointXYZ> & cloud1,
+									   pcl::PointCloud<pcl::PointXYZ> & cloud2);
+
+void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
+							   const pcl::PointCloud<pcl::PointXYZ> & cloud1,
+							   const pcl::PointCloud<pcl::PointXYZ> & cloud2,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers1,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers2,
+							   char depthAxis);
+void RTABMAP_EXP extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
+							   const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
+							   const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers1,
+							   pcl::PointCloud<pcl::PointXYZ> & inliers2,
+							   char depthAxis);
+
+int RTABMAP_EXP countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
+					 const std::multimap<int, pcl::PointXYZ> & wordsB);
+
+void RTABMAP_EXP filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
+					pcl::PointCloud<pcl::PointXYZ> & inliers2,
+					float maxDepth,
+					char depthAxis,
+					bool removeDuplicates);
+
+Transform RTABMAP_EXP transformFromXYZCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
+		double inlierThreshold = 0.02,
+		int iterations = 100,
+		int * inliers = 0);
+
+Transform RTABMAP_EXP icp(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		double & fitnessScore);
+
+Transform RTABMAP_EXP icpPointToPlane(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		double & fitnessScore);
+
+Transform RTABMAP_EXP icp2D(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		double & fitnessScore);
+
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+
+int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+							const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+							float maxDistance);
+
+void RTABMAP_EXP findCorrespondences(
+		const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs);
+
+void RTABMAP_EXP findCorrespondences(
+		const std::multimap<int, pcl::PointXYZ> & words1,
+		const std::multimap<int, pcl::PointXYZ> & words2,
+		pcl::PointCloud<pcl::PointXYZ> & inliers1,
+		pcl::PointCloud<pcl::PointXYZ> & inliers2,
+		float maxDepth);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform = Transform::getIdentity());
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
+		const cv::Mat & depth,
+		float depthConstant,
+		int decimation,
+		double maxDepth,
+		float voxel,
+		int samples,
+		const Transform & transform = Transform::getIdentity());
+
+inline Eigen::Matrix4f transformToEigen4f(const Transform & transform)
+{
+	Eigen::Matrix4f m;
+	m << transform[0], transform[1], transform[2], transform[3],
+		 transform[4], transform[5], transform[6], transform[7],
+		 transform[8], transform[9], transform[10], transform[11],
+		 0,0,0,1;
+	return m;
+}
+inline Eigen::Matrix4d transformToEigen4d(const Transform & transform)
+{
+	Eigen::Matrix4d m;
+	m << transform[0], transform[1], transform[2], transform[3],
+		 transform[4], transform[5], transform[6], transform[7],
+		 transform[8], transform[9], transform[10], transform[11],
+		 0,0,0,1;
+	return m;
+}
+
+inline Eigen::Affine3f transformToEigen3f(const Transform & transform)
+{
+	return Eigen::Affine3f(transformToEigen4f(transform));
+}
+
+inline Eigen::Affine3d transformToEigen3d(const Transform & transform)
+{
+	return Eigen::Affine3d(transformToEigen4d(transform));
+}
+
+inline Transform transformFromEigen4f(const Eigen::Matrix4f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+inline Transform transformFromEigen4d(const Eigen::Matrix4d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
+inline Transform transformFromEigen3f(const Eigen::Affine3f & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+inline Transform transformFromEigen3d(const Eigen::Affine3d & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP get3DFASTKpts(
+		const cv::Mat & image,
+		const cv::Mat & imageDepth,
+		float constant,
+		int fastThreshold=50,
+		bool fastNonmaxSuppression=true,
+		float maxDepth = 5.0f);
+
+pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float maxEdgeLength = 0.025, bool smoothing = true);
+
+void RTABMAP_EXP optimizeTOROGraph(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, std::pair<int, Transform> > & edgeConstraints,
+		int toroIterations,
+		std::map<int, Transform> & optimizedPoses,
+		Transform & mapCorrection);
+
+bool RTABMAP_EXP saveTOROGraph(
+		const std::string & fileName,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
+
+bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
+		std::map<int, Transform> & poses,
+		std::multimap<int, std::pair<int, Transform> > & edgeConstraints);
+
+} // namespace util3d
+} // namespace rtabmap
+
+#endif /* UTIL3D_H_ */