Refactored OdometryBOW class to handle binary descriptors too (removed OdometryBin).

Odometry: added "local map history" option to increase precision
Added new Nearest neighbor options (LSH, brute Force, GPU brute Force)
Added FREAK/ORB features
Increased version to 0.6.5

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

corelib/include/rtabmap/core/CameraEvent.h

@@ -44,9 +44,9 @@ public:
 		_image(image, seq)
 	{
 	}
-	CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int seq=0) :
+	CameraEvent(const cv::Mat & descriptors, Feature2D::Type featureType, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int seq=0) :
 		UEvent(kCodeFeatures),
-		_image(image, seq, descriptors, keypoints)
+		_image(image, seq, descriptors, featureType, keypoints)
 	{
 	}
 	CameraEvent() :

corelib/include/rtabmap/core/Features2d.h

@@ -28,6 +28,16 @@
 #include <list>
 #include "rtabmap/core/Parameters.h"
 
+namespace cv{
+class SURF;
+class SIFT;
+namespace gpu {
+	class SURF_GPU;
+	class ORB_GPU;
+	class FAST_GPU;
+}
+}
+
 namespace rtabmap {
 
 void RTABMAP_EXP filterKeypointsByDepth(
@@ -45,119 +55,115 @@ void RTABMAP_EXP filterKeypointsByDepth(
 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
-/////////////////////
-class RTABMAP_EXP KeypointDescriptor {
+cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
+
+// Feature2D
+class RTABMAP_EXP Feature2D {
 public:
-	enum DescriptorType {kDescriptorSurf, kDescriptorSift, kDescriptorUndef};
+	enum Type {kFeatureUndef=-1, kFeatureSurf=0, kFeatureSift=1, kFeatureOrb=2, kFeatureFastFreak=3, kFeatureFastBrief=4};
 
 public:
-	virtual ~KeypointDescriptor();
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
+	virtual ~Feature2D() {}
+
+	std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, int maxKeypoints=0, const cv::Rect & roi = cv::Rect()) const;
+	cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 protected:
-	KeypointDescriptor(const ParametersMap & parameters = ParametersMap());
+	Feature2D(const ParametersMap & parameters = ParametersMap()) {}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const = 0;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
 };
 
-//SURFDescriptor
-class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
+//SURF
+class RTABMAP_EXP SURF : public Feature2D
 {
 public:
-	SURFDescriptor(const ParametersMap & parameters = ParametersMap());
-	virtual ~SURFDescriptor();
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	SURF(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF();
 
 private:
-	double _hessianThreshold;
-	int _nOctaves;
-	int _nOctaveLayers;
-	bool _extended;
-	bool _upright;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
-	bool _gpuVersion;
+private:
+	cv::SURF * _surf;
+	cv::gpu::SURF_GPU * _gpuSurf;
 };
 
-//SIFTDescriptor
-class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
+//SIFT
+class RTABMAP_EXP SIFT : public Feature2D
 {
 public:
-	SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
-	virtual ~SIFTDescriptor();
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+	SIFT(const ParametersMap & parameters = ParametersMap());
+	virtual ~SIFT();
 
 private:
-	int _nfeatures;
-	int _nOctaveLayers;
-	double _contrastThreshold;
-	double _edgeThreshold;
-	double _sigma;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	cv::SIFT * _sift;
 };
 
-
-/////////////////////
-// KeypointDetector
-/////////////////////
-class RTABMAP_EXP KeypointDetector
+//ORB
+class RTABMAP_EXP ORB : public Feature2D
 {
 public:
-	enum DetectorType {kDetectorSurf, kDetectorSift, kDetectorUndef};
+	ORB(const ParametersMap & parameters = ParametersMap());
+	virtual ~ORB();
 
-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,
-			int maxKeypoints = 0,
-			const cv::Rect & roi = cv::Rect());
-	virtual void parseParameters(const ParametersMap & parameters);
-	void setRoi(const std::string & roi);
-
-protected:
-	KeypointDetector(const ParametersMap & parameters = ParametersMap());
 private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	cv::ORB * _orb;
+	cv::gpu::ORB_GPU * _gpuOrb;
 };
 
-//SURFDetector
-class RTABMAP_EXP SURFDetector : public KeypointDetector
+//FAST
+class RTABMAP_EXP FAST : public Feature2D
 {
 public:
-	SURFDetector(const ParametersMap & parameters = ParametersMap());
-	virtual ~SURFDetector();
-	virtual void parseParameters(const ParametersMap & parameters);
-private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
-private:
-	double _hessianThreshold;
-	int _nOctaves;
-	int _nOctaveLayers;
-	bool _extended;
-	bool _upright;
+	FAST(const ParametersMap & parameters = ParametersMap());
+	virtual ~FAST();
 
-	bool _gpuVersion;
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
+
+private:
+	cv::FastFeatureDetector * _fast;
+	cv::gpu::FAST_GPU * _gpuFast;
 };
 
-//SIFTDetector
-class RTABMAP_EXP SIFTDetector : public KeypointDetector
+//FAST_BRIEF
+class RTABMAP_EXP FAST_BRIEF : public FAST
 {
 public:
-	SIFTDetector(const ParametersMap & parameters = ParametersMap());
-	virtual ~SIFTDetector();
-	virtual void parseParameters(const ParametersMap & parameters);
+	FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
+	virtual ~FAST_BRIEF();
+
 private:
-	virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
 private:
-	int _nfeatures;
-	int _nOctaveLayers;
-	double _contrastThreshold;
-	double _edgeThreshold;
-	double _sigma;
+	cv::BriefDescriptorExtractor * _brief;
+};
+
+//FAST_FREAK
+class RTABMAP_EXP FAST_FREAK : public FAST
+{
+public:
+	FAST_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~FAST_FREAK();
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	cv::FREAK * _freak;
 };
 
 }

corelib/include/rtabmap/core/Image.h

@@ -12,6 +12,7 @@
 #include <opencv2/features2d/features2d.hpp>
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Features2d.h>
 
 namespace rtabmap
 {
@@ -25,10 +26,12 @@ public:
 	Image(const cv::Mat & image = cv::Mat(),
 		  int id = 0,
 		  const cv::Mat & descriptors = cv::Mat(),
+		  Feature2D::Type featureType = Feature2D::kFeatureUndef,
 		  const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>()) :
 		_image(image),
 		_id(id),
 		_descriptors(descriptors),
+		_featureType(featureType),
 		_keypoints(keypoints),
 		_depthConstant(0.0f),
 		_localTransform(Transform::getIdentity())
@@ -44,6 +47,7 @@ public:
 		  int id = 0) :
 		_image(image),
 		_id(id),
+		_featureType(Feature2D::kFeatureUndef),
 		_depth(depth),
 		_depthConstant(depthConstant),
 		_pose(pose),
@@ -61,6 +65,7 @@ public:
 		  int id = 0) :
 		_image(image),
 		_id(id),
+		_featureType(Feature2D::kFeatureUndef),
 		_depth(depth),
 		_depth2d(depth2d),
 		_depthConstant(depthConstant),
@@ -75,8 +80,9 @@ public:
 	const cv::Mat & image() const {return _image;}
 	int id() const {return _id;};
 	const cv::Mat & descriptors() const {return _descriptors;}
+	Feature2D::Type featureType() const {return _featureType;}
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
-	void setDescriptors(const cv::Mat & descriptors) {_descriptors = descriptors;}
+	void setDescriptors(const cv::Mat & descriptors, Feature2D::Type featureType) {_descriptors = descriptors; _featureType=featureType;}
 	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
 
 	bool isMetric() const {return !_depth.empty() || _depthConstant != 0.0f || !_pose.isNull();}
@@ -91,6 +97,7 @@ private:
 	cv::Mat _image;
 	int _id;
 	cv::Mat _descriptors;
+	Feature2D::Type _featureType;
 	std::vector<cv::KeyPoint> _keypoints;
 
 	// Metric stuff

corelib/include/rtabmap/core/Memory.h

@@ -41,8 +41,7 @@ class DBDriver;
 class GraphNode;
 class VWDictionary;
 class VisualWord;
-class KeypointDetector;
-class KeypointDescriptor;
+class Feature2D;
 class Statistics;
 
 class RTABMAP_EXP Memory
@@ -80,7 +79,7 @@ public:
 			bool incrementMarginOnLoop = false,
 			bool ignoreLoopIds = false,
 			double * dbAccessTime = 0) const;
-	void deleteLocation(int locationId);
+	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
 	void rejectLoopClosure(int oldId, int newId);
 
 	//getters
@@ -135,8 +134,9 @@ public:
 			const std::set<int> & endIds = std::set<int>());
 
 	//keypoint stuff
-	int getVWDictionarySize() const;
+	const VWDictionary * getVWDictionary() const;
 	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
+	Feature2D::Type getFeatureType() const {return _featureType;}
 	void extractKeypointsAndDescriptors(
 			const cv::Mat & image,
 			const cv::Mat & depth,
@@ -144,6 +144,7 @@ public:
 			std::vector<cv::KeyPoint> & keypoints,
 			cv::Mat & descriptors);
 
+	// RGB-D stuff
 	void getMetricConstraints(
 			const std::vector<int> & ids,
 			std::map<int, Transform> & poses,
@@ -162,7 +163,7 @@ private:
 	void preUpdate();
 	void addSignatureToStm(Signature * signature);
 	void clear();
-	void moveToTrash(Signature * s, bool saveToDatabase = true);
+	void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);
 
 	void addSignatureToWm(Signature * signature);
 	Signature * _getSignature(int id) const;
@@ -181,7 +182,7 @@ private:
 			bool keepRawData=false);
 
 	//keypoint stuff
-	void disableWordsRef(int signatureId, bool saveToDatabase = true);
+	void disableWordsRef(int signatureId);
 	void enableWordsRef(const std::list<int> & signatureIds);
 	void cleanUnusedWords();
 	int getNi(int signatureId) const;
@@ -215,8 +216,8 @@ private:
 
 	//Keypoint stuff
 	VWDictionary * _vwd;
-	KeypointDetector * _keypointDetector;
-	KeypointDescriptor * _keypointDescriptor;
+	Feature2D * _feature2D;
+	Feature2D::Type _featureType;
 	float _badSignRatio;;
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;

corelib/include/rtabmap/core/Odometry.h

@@ -40,6 +40,7 @@ public:
 	bool isLargeEnoughTransform(const Transform & transform);
 
 	//getters
+	const Transform & getPose() const {return _pose;}
 	int getMaxFeatures() const  {return _maxFeatures;}
 	int getMinInliers() const {return _minInliers;}
 	float getInlierDistance() const {return _inlierDistance;}
@@ -48,6 +49,7 @@ public:
 	float getMaxDepth() const {return _maxDepth;}
 	float geLinearUpdate() const {return _linearUpdate;}
 	float getAngularUpdate() const {return _angularUpdate;}
+	int getLocalHistory() const {return _localHistory;}
 
 private:
 	virtual Transform computeTransform(Image & image, int * quality = 0) = 0;
@@ -62,76 +64,20 @@ private:
 	float _linearUpdate;
 	float _angularUpdate;
 	int _resetCountdown;
+	int _localHistory;
 	Transform _pose;
 	int _resetCurrentCount;
 
 protected:
-	Odometry(float inlierDistance = Parameters::defaultOdomInlierDistance(),
-			int maxWords = Parameters::defaultOdomMaxWords(),
-			int minInliers = Parameters::defaultOdomMinInliers(),
-			int iterations = Parameters::defaultOdomIterations(),
-			float wordsRatio = Parameters::defaultOdomWordsRatio(),
-			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 wordsRatio = Parameters::defaultOdomWordsRatio(),
-			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, int * quality = 0);
-
-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 wordsRatio = Parameters::defaultOdomWordsRatio(),
-			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);
+	OdometryBOW(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
 	virtual ~OdometryBOW();
 
 	virtual void reset();
@@ -141,23 +87,19 @@ private:
 
 private:
 	Memory * _memory;
+	std::multimap<int, pcl::PointXYZ> localMap_;
 };
 
 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);
+	OdometryICP(int decimation = 4,
+			float voxelSize = 0.005f,
+			int samples = 0,
+			float maxCorrespondenceDistance = 0.05f,
+			int maxIterations = 30,
+			float maxFitness = 0.01f,
+			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
 	void reset();
 
 private:

corelib/include/rtabmap/core/Parameters.h

@@ -153,17 +153,13 @@ class RTABMAP_EXP Parameters
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, PublishKeypoints,      bool, true, 		"Publishing keypoints.");
-	RTABMAP_PARAM(Kp, NNStrategy,            int, 1, 	 		"Naive 0, kdForest 1.");
+	RTABMAP_PARAM(Kp, NNStrategy,            int, 1, 	 		"kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
 	RTABMAP_PARAM(Kp, MaxDepth,              float, 0.0, 		"Filter extracted keypoints by depth (0=inf)");
 	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, DetectorStrategy,      int, 0, 			"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
 	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].");
@@ -183,6 +179,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(SURF, OctaveLayers, 	  int, 2, 		"");
 	RTABMAP_PARAM(SURF, Upright, 	      bool, false, 	"U-SURF");
 	RTABMAP_PARAM(SURF, GpuVersion, 	  bool, false, 	"");
+	RTABMAP_PARAM(SURF, GpuKeypointsRatio, 	  float, 0.01, 	"");
 
 	RTABMAP_PARAM(SIFT, NFeatures,          int, 0,		"");
 	RTABMAP_PARAM(SIFT, NOctaveLayers,      int, 3, 	"");
@@ -190,6 +187,28 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(SIFT, EdgeThreshold,      double, 10.0, "");
 	RTABMAP_PARAM(SIFT, Sigma,              double, 1.6, "");
 
+	RTABMAP_PARAM(BRIEF, Bytes,         	   int, 32, 	"Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
+
+	RTABMAP_PARAM(FAST, Threshold,          int, 30, 	   "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
+	RTABMAP_PARAM(FAST, NonmaxSuppression,  bool, true, 	"If true, non-maximum suppression is applied to detected corners (keypoints).");
+	RTABMAP_PARAM(FAST, Gpu,                bool, false, 	"GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
+	RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, 	"Used with FAST GPU.");
+
+	RTABMAP_PARAM(ORB, NFeatures,            int, 500,     "The maximum number of features to retain.");
+	RTABMAP_PARAM(ORB, ScaleFactor,          float,  1.2, "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
+	RTABMAP_PARAM(ORB, NLevels,              int, 8,       "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
+	RTABMAP_PARAM(ORB, EdgeThreshold,        int, 31,      "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
+	RTABMAP_PARAM(ORB, FirstLevel,           int, 0,       "It should be 0 in the current implementation.");
+	RTABMAP_PARAM(ORB, WTA_K,                int, 2,       "The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).");
+	RTABMAP_PARAM(ORB, ScoreType,            int, 0,       "The default HARRIS_SCORE=0 means that Harris algorithm is used to rank features (the score is written to KeyPoint::score and is used to retain best nfeatures features); FAST_SCORE=1 is alternative value of the parameter that produces slightly less stable keypoints, but it is a little faster to compute.");
+	RTABMAP_PARAM(ORB, PatchSize,            int, 31,      "size of the patch used by the oriented BRIEF descriptor. Of course, on smaller pyramid layers the perceived image area covered by a feature will be larger.");
+	RTABMAP_PARAM(ORB, Gpu,                  bool, false, "GPU-ORB: Use GPU version of ORB. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
+
+	RTABMAP_PARAM(FREAK, OrientationNormalized, bool, true,   "Enable orientation normalization.");
+	RTABMAP_PARAM(FREAK, ScaleNormalized,       bool, true,   "Enable scale normalization.");
+	RTABMAP_PARAM(FREAK, PatternScale,          float, 22.0,  "Scaling of the description pattern.");
+	RTABMAP_PARAM(FREAK, NOctaves,              int, 4,        "Number of octaves covered by the detected keypoints.");
+
 	// 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, ...}.");
@@ -216,7 +235,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID,   int, 0,      "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
 
 	// Odometry
-	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=BOW 1=Binary.");
+	RTABMAP_PARAM(Odom, Type,           		int, 0, 		"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
 	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.");
@@ -225,19 +244,10 @@ class RTABMAP_EXP Parameters
 	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, WordsRatio,             float, 0.5, 	"Minmum ratio of keypoints between the current image and the last image to compute odometry.");
-	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, 	"");
+	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(Odom, LocalHistory,           int, 0,         "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
+	RTABMAP_PARAM(Odom, NearestNeighbor,        int, 1, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
+	RTABMAP_PARAM(Odom, NNDR,                   float, 0.7,     "NNDR: nearest neighbor distance ratio.");
 
 	// Loop closure constraint
 	RTABMAP_PARAM(LccIcp, Type,            int, 0, 			"0=No ICP, 1=ICP 3D, 2=ICP 2D");

corelib/include/rtabmap/core/VWDictionary.h

@@ -31,14 +31,13 @@
 namespace rtabmap
 {
 
-class FlannNN;
 class DBDriver;
 class VisualWord;
 
 class RTABMAP_EXP VWDictionary
 {
 public:
-	enum NNStrategy{kNNNaive, kNNFlannKdTree, kNNUndef};
+	enum NNStrategy{kNNFlannNaive, kNNFlannKdTree, kNNFlannLSH, kNNBruteForce, kNNBruteForceGPU, kNNUndef};
 	static const int ID_START;
 	static const int ID_INVALID;
 
@@ -55,34 +54,28 @@ public:
 			int signatureId);
 	virtual void addWord(VisualWord * vw);
 
-	virtual std::vector<int> findNN(const std::list<VisualWord *> & vws, bool searchInNewlyAddedWords = true) const;
-	void naiveNNSearch(const std::list<VisualWord *> & words, const float * d, int length, std::map<float, int> & results, unsigned int k) const;
+	virtual std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
 
 	void addWordRef(int wordId, int signatureId);
 	void removeAllWordRef(int wordId, int signatureId);
 	const VisualWord * getWord(int id) const;
 	VisualWord * getUnusedWord(int id) const;
 	void setLastWordId(int id) {_lastWordId = id;}
-	void getCommonWords(unsigned int nbCommonWords, int totalSign, std::list<int> & commonWords) const;
 	const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
-	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;}
 	float getNndrRatio() {return _nndrRatio;}
 	unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();}
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
-	void setNNStrategy(NNStrategy strategy, const ParametersMap & parameters = ParametersMap());
-	NNStrategy nnStrategy() const;
+	void setNNStrategy(NNStrategy strategy);
 	bool isIncremental() const {return _incrementalDictionary;}
-	void setIncrementalDictionary(bool incrementalDictionary, const std::string & dictionaryPath);
+	void setIncrementalDictionary();
+	void setFixedDictionary(const std::string & dictionaryPath);
 
 	void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
 
 	void clear();
 	std::vector<VisualWord *> getUnusedWords() const;
+	std::vector<int> getUnusedWordIds() const;
 	unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();}
 	void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
 
@@ -95,16 +88,12 @@ protected:
 
 private:
 	bool _incrementalDictionary;
-	bool _minDistUsed;
-	float _minDist; //euclidean distance ^ 2
-	bool _nndrUsed;
 	float _nndrRatio;
-	unsigned int _maxLeafs;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt)
-	int _dim;
 	int _lastWordId;
-	FlannNN * _nn;
+	cv::flann::Index * _flannIndex;
 	cv::Mat _dataTree;
+	NNStrategy _strategy;
 	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

corelib/include/rtabmap/core/util3d.h

@@ -11,6 +11,7 @@
 #include <opencv2/features2d/features2d.hpp>
 #include <list>
 #include <string>
+#include <set>
 
 #include <rtabmap/core/Link.h>
 #include <rtabmap/utilite/UThread.h>
@@ -142,6 +143,13 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & transform);
 
+pcl::PointXYZ RTABMAP_EXP transformPoint(
+		const pcl::PointXYZ & pt,
+		const Transform & transform);
+pcl::PointXYZRGB RTABMAP_EXP transformPoint(
+		const pcl::PointXYZRGB & pt,
+		const Transform & transform);
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float depthConstant,
@@ -273,7 +281,8 @@ void RTABMAP_EXP findCorrespondences(
 		const std::multimap<int, pcl::PointXYZ> & words2,
 		pcl::PointCloud<pcl::PointXYZ> & inliers1,
 		pcl::PointCloud<pcl::PointXYZ> & inliers2,
-		float maxDepth);
+		float maxDepth,
+		std::set<int> * uniqueCorrespondences = 0);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
 		const cv::Mat & matrix,