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

CMakeLists.txt

@@ -16,7 +16,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 6)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 5)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -132,7 +132,7 @@ IF(APPLE)
 ENDIF(APPLE)
 
 ####### DEPENDENCIES #######
-FIND_PACKAGE(OpenCV REQUIRED core highgui flann features2d calib3d imgproc nonfree)
+FIND_PACKAGE(OpenCV REQUIRED core highgui flann features2d calib3d imgproc nonfree gpu)
 FIND_PACKAGE(PCL 1.7 REQUIRED)
 FIND_PACKAGE(VTK REQUIRED)
 FIND_PACKAGE(ZLIB REQUIRED)

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);
 
-/////////////////////
+cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
-// KeypointDescriptor
+
-/////////////////////
+// Feature2D
-class RTABMAP_EXP KeypointDescriptor {
+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 ~Feature2D() {}
-	virtual void parseParameters(const ParametersMap & parameters);
+
-	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
+	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
+//SURF
-class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
+class RTABMAP_EXP SURF : public Feature2D
 {
 public:
-	SURFDescriptor(const ParametersMap & parameters = ParametersMap());
+	SURF(const ParametersMap & parameters = ParametersMap());
-	virtual ~SURFDescriptor();
+	virtual ~SURF();
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	double _hessianThreshold;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
-	int _nOctaves;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
-	int _nOctaveLayers;
-	bool _extended;
-	bool _upright;
 
-	bool _gpuVersion;
+private:
+	cv::SURF * _surf;
+	cv::gpu::SURF_GPU * _gpuSurf;
 };
 
-//SIFTDescriptor
+//SIFT
-class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
+class RTABMAP_EXP SIFT : public Feature2D
 {
 public:
-	SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
+	SIFT(const ParametersMap & parameters = ParametersMap());
-	virtual ~SIFTDescriptor();
+	virtual ~SIFT();
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	int _nfeatures;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
-	int _nOctaveLayers;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
-	double _contrastThreshold;
+
-	double _edgeThreshold;
+private:
-	double _sigma;
+	cv::SIFT * _sift;
 };
 
-
+//ORB
-/////////////////////
+class RTABMAP_EXP ORB : public Feature2D
-// KeypointDetector
-/////////////////////
-class RTABMAP_EXP KeypointDetector
 {
 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
+//FAST
-class RTABMAP_EXP SURFDetector : public KeypointDetector
+class RTABMAP_EXP FAST : public Feature2D
 {
 public:
-	SURFDetector(const ParametersMap & parameters = ParametersMap());
+	FAST(const ParametersMap & parameters = ParametersMap());
-	virtual ~SURFDetector();
+	virtual ~FAST();
-	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;
 
-	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
+//FAST_BRIEF
-class RTABMAP_EXP SIFTDetector : public KeypointDetector
+class RTABMAP_EXP FAST_BRIEF : public FAST
 {
 public:
-	SIFTDetector(const ParametersMap & parameters = ParametersMap());
+	FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
-	virtual ~SIFTDetector();
+	virtual ~FAST_BRIEF();
-	virtual void parseParameters(const ParametersMap & parameters);
+
 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;
+	cv::BriefDescriptorExtractor * _brief;
-	int _nOctaveLayers;
+};
-	double _contrastThreshold;
+
-	double _edgeThreshold;
+//FAST_FREAK
-	double _sigma;
+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 Feature2D;
-class KeypointDescriptor;
 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;
+	Feature2D * _feature2D;
-	KeypointDescriptor * _keypointDescriptor;
+	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(
+	OdometryBOW(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-			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);
 	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(
+	OdometryICP(int decimation = 4,
-			int decimation = Parameters::defaultOdomICPDecimation(),
+			float voxelSize = 0.005f,
-			float voxelSize = Parameters::defaultOdomICPVoxelSize(),
+			int samples = 0,
-			float samples = Parameters::defaultOdomICPSamples(),
+			float maxCorrespondenceDistance = 0.05f,
-			float maxCorrespondenceDistance = Parameters::defaultOdomICPCorrespondencesDistance(),
+			int maxIterations = 30,
-			int	maxIterations = Parameters::defaultOdomICPIterations(),
+			float maxFitness = 0.01f,
-			float maxFitness = Parameters::defaultOdomICPMaxFitness(),
+			const ParametersMap & odometryParameter = rtabmap::ParametersMap());
-			float maxDepth = Parameters::defaultOdomMaxDepth(),
-			float linearUpdate = Parameters::defaultOdomLinearUpdate(),
-			float angularUpdate = Parameters::defaultOdomAngularUpdate(),
-			int resetCoutdown = Parameters::defaultOdomResetCountdown());
-	OdometryICP(const ParametersMap & parameters);
 	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, 			"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
-	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].");
@@ -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(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(OdomBin, BriefBytes,         	   int, 32, 	"");
+	RTABMAP_PARAM(Odom, NearestNeighbor,        int, 1, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
-	RTABMAP_PARAM(OdomBin, FastThreshold,          int, 30, 	"");
+	RTABMAP_PARAM(Odom, NNDR,                   float, 0.7,     "NNDR: nearest neighbor distance ratio.");
-	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, 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;
+	virtual std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
-	void naiveNNSearch(const std::list<VisualWord *> & words, const float * d, int length, std::map<float, int> & results, unsigned int k) const;
 
 	void addWordRef(int wordId, int signatureId);
 	void removeAllWordRef(int wordId, int signatureId);
 	const VisualWord * getWord(int id) const;
 	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());
+	void setNNStrategy(NNStrategy strategy);
-	NNStrategy nnStrategy() const;
 	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,

corelib/src/CMakeLists.txt

@@ -22,7 +22,6 @@ SET(SRC_FILES
 	Parameters.cpp
     Signature.cpp
 	Features2d.cpp
-	NearestNeighbor.cpp
 	Transform.cpp
 	
 	util3d.cpp

corelib/src/DBDriverSqlite3.cpp

@@ -1172,7 +1172,9 @@ void DBDriverSqlite3::loadQuery(VWDictionary * dictionary) const
 				{
 					UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
 				}
-				VisualWord * vw = new VisualWord(id, &((const float *)descriptor)[0], descriptorSize, 0);
+				cv::Mat d(1, descriptorSize, CV_32F);
+				memcpy(d.data, descriptor, dRealSize);
+				VisualWord * vw = new VisualWord(id, d);
 				vw->setSaved(true);
 				dictionary->addWord(vw);
 			}
@@ -1244,7 +1246,9 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 					UERROR("Saved buffer size (%d) is not the same as descriptor size (%d)", dRealSize/sizeof(float), descriptorSize);
 				}
 
-				VisualWord * vw = new VisualWord(*iter, &((const float *)descriptor)[0], descriptorSize);
+				cv::Mat d(1, descriptorSize, CV_32F);
+				memcpy(d.data, descriptor, dRealSize);
+				VisualWord * vw = new VisualWord(*iter, d);
 				if(vw)
 				{
 					vw->setSaved(true);
@@ -1739,9 +1743,9 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
 				{
 					rc = sqlite3_bind_int(ppStmt, 1, w->id());
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-					rc = sqlite3_bind_int(ppStmt, 2, w->getDim());
+					rc = sqlite3_bind_int(ppStmt, 2, w->getDescriptor().cols);
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
-					rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor(), w->getDim()*sizeof(float), SQLITE_STATIC);
+					rc = sqlite3_bind_blob(ppStmt, 3, w->getDescriptor().data, w->getDescriptor().cols*sizeof(float), SQLITE_STATIC);
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 
 					//execute query

corelib/src/Features2d.cpp

@@ -144,218 +144,7 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
 	}
 }
 
-/////////////////////
+cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
-// KeypointDescriptor
-/////////////////////
-KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
-{
-	this->parseParameters(parameters);
-}
-
-KeypointDescriptor::~KeypointDescriptor()
-{
-}
-
-void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
-{
-}
-
-//////////////////////////
-//SURFDescriptor
-//////////////////////////
-SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
-	KeypointDescriptor(parameters),
-	_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
-	_nOctaves(Parameters::defaultSURFOctaves()),
-	_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
-	_extended(Parameters::defaultSURFExtended()),
-	_upright(Parameters::defaultSURFUpright()),
-	_gpuVersion(Parameters::defaultSURFGpuVersion())
-{
-	this->parseParameters(parameters);
-}
-
-SURFDescriptor::~SURFDescriptor()
-{
-}
-
-void SURFDescriptor::parseParameters(const ParametersMap & parameters)
-{
-	Parameters::parse(parameters, Parameters::kSURFExtended(), _extended);
-	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), _hessianThreshold);
-	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), _nOctaveLayers);
-	Parameters::parse(parameters, Parameters::kSURFOctaves(), _nOctaves);
-	Parameters::parse(parameters, Parameters::kSURFUpright(), _upright);
-	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), _gpuVersion);
-	KeypointDescriptor::parseParameters(parameters);
-}
-
-cv::Mat SURFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
-{
-	ULOGGER_DEBUG("");
-	cv::Mat descriptors;
-	if(image.empty())
-	{
-		ULOGGER_ERROR("Image is null ?!?");
-		return descriptors;
-	}
-	// SURF support only grayscale images
-	cv::Mat imageGrayScale;
-	if(image.channels() != 1 || image.depth() != CV_8U)
-	{
-		cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
-	}
-	cv::Mat img;
-	if(!imageGrayScale.empty())
-	{
-		img = imageGrayScale;
-	}
-	else
-	{
-		img =  image;
-	}
-	if(_gpuVersion && cv::gpu::getCudaEnabledDeviceCount())
-	{
-		std::vector<float> d;
-		cv::gpu::GpuMat imgGpu(img);
-		cv::gpu::GpuMat descriptorsGpu;
-		cv::gpu::GpuMat keypointsGpu;
-		cv::gpu::SURF_GPU surfGpu(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, 0.01f, _upright);
-		surfGpu.uploadKeypoints(keypoints, keypointsGpu);
-		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
-		surfGpu.downloadDescriptors(descriptorsGpu, d);
-		unsigned int dim = _extended?128:64;
-		descriptors = cv::Mat(d.size()/dim, dim, CV_32F);
-		for(int i=0; i<descriptors.rows; ++i)
-		{
-			float * rowFl = descriptors.ptr<float>(i);
-			memcpy(rowFl, &d[i*dim], dim*sizeof(float));
-		}
-	}
-	else
-	{
-		if(_gpuVersion)
-		{
-			UWARN("GPU version of SURF not available! Using CPU version instead...");
-		}
-
-		cv::SURF extractor(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
-		extractor.compute(img, keypoints, descriptors);
-	}
-
-	return descriptors;
-}
-
-//////////////////////////
-//SIFTDescriptor
-//////////////////////////
-SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
-	KeypointDescriptor(parameters),
-	_nfeatures(Parameters::defaultSIFTNFeatures()),
-	_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
-	_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
-	_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
-	_sigma(Parameters::defaultSIFTSigma())
-{
-	this->parseParameters(parameters);
-}
-
-SIFTDescriptor::~SIFTDescriptor()
-{
-}
-
-void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
-{
-	ParametersMap::const_iterator iter;
-	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), _contrastThreshold);
-	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), _edgeThreshold);
-	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), _nfeatures);
-	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), _nOctaveLayers);
-	Parameters::parse(parameters, Parameters::kSIFTSigma(), _sigma);
-	KeypointDescriptor::parseParameters(parameters);
-}
-
-cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
-{
-	ULOGGER_DEBUG("");
-	cv::Mat descriptors;
-	if(image.empty())
-	{
-		ULOGGER_ERROR("Image is null ?!?");
-		return descriptors;
-	}
-	// SURF support only grayscale images
-	cv::Mat imageGrayScale;
-	if(image.channels() != 1 || image.depth() != CV_8U)
-	{
-		cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
-	}
-	cv::Mat img;
-	if(!imageGrayScale.empty())
-	{
-		img = imageGrayScale;
-	}
-	else
-	{
-		img =  image;
-	}
-
-	cv::SIFT extractor(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
-	extractor.compute(img, keypoints, descriptors);
-
-	return descriptors;
-}
-
-
-
-
-/////////////////////
-// KeypointDetector
-/////////////////////
-KeypointDetector::KeypointDetector(const ParametersMap & parameters)
-{
-	this->parseParameters(parameters);
-}
-
-void KeypointDetector::parseParameters(const ParametersMap & parameters)
-{
-}
-
-std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(
-		const cv::Mat & image,
-		int maxKeypoints,
-		const cv::Rect & roi)
-{
-	ULOGGER_DEBUG("");
-	std::vector<cv::KeyPoint> keypoints;
-	if(!image.empty())
-	{
-		UTimer timer;
-
-		// Get keypoints
-		keypoints = this->_generateKeypoints(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
-		ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
-
-		limitKeypoints(keypoints, maxKeypoints);
-
-		if(roi.x || roi.y)
-		{
-			// Adjust keypoint position to raw image
-			for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
-			{
-				iter->pt.x += roi.x;
-				iter->pt.y += roi.y;
-			}
-		}
-	}
-	else
-	{
-		ULOGGER_ERROR("Image is null!");
-	}
-	return keypoints;
-}
-
-cv::Rect KeypointDetector::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
 	if(!image.empty() && roiRatios.size() == 4)
 	{
@@ -401,144 +190,426 @@ cv::Rect KeypointDetector::computeRoi(const cv::Mat & image, const std::vector<f
 	}
 }
 
-
+/////////////////////
-//////////////////////////
+// Feature2D
-//SURFDetector
+/////////////////////
-//////////////////////////
+std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, int maxKeypoints, const cv::Rect & roi) const
-SURFDetector::SURFDetector(const ParametersMap & parameters) :
-		KeypointDetector(parameters),
-		_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
-		_nOctaves(Parameters::defaultSURFOctaves()),
-		_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
-		_extended(Parameters::defaultSURFExtended()),
-		_upright(Parameters::defaultSURFUpright()),
-		_gpuVersion(Parameters::defaultSURFGpuVersion())
-{
-	this->parseParameters(parameters);
-}
-
-SURFDetector::~SURFDetector()
-{
-}
-
-void SURFDetector::parseParameters(const ParametersMap & parameters)
-{
-	Parameters::parse(parameters, Parameters::kSURFExtended(), _extended);
-	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), _hessianThreshold);
-	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), _nOctaveLayers);
-	Parameters::parse(parameters, Parameters::kSURFOctaves(), _nOctaves);
-	Parameters::parse(parameters, Parameters::kSURFUpright(), _upright);
-	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), _gpuVersion);
-	KeypointDetector::parseParameters(parameters);
-}
-
-std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
 {
 	ULOGGER_DEBUG("");
 	std::vector<cv::KeyPoint> keypoints;
-	if(image.empty())
+	if(!image.empty() && image.channels() == 1 && image.type() == CV_8U)
 	{
-		ULOGGER_ERROR("Image is null ?!?");
+		UTimer timer;
-		return keypoints;
+
+		// Get keypoints
+		keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
+		ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
+
+		limitKeypoints(keypoints, maxKeypoints);
+
+		if(roi.x || roi.y)
+		{
+			// Adjust keypoint position to raw image
+			for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
+			{
+				iter->pt.x += roi.x;
+				iter->pt.y += roi.y;
+			}
+		}
 	}
-	// SURF support only grayscale images
+	else if(image.empty())
-	cv::Mat imageGrayScale;
-	if(image.channels() != 1 || image.depth() != CV_8U)
 	{
-		ULOGGER_DEBUG("");
+		UERROR("Image is null!");
-		cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
-	}
-	cv::Mat img;
-	if(!imageGrayScale.empty())
-	{
-		img = imageGrayScale;
 	}
 	else
 	{
-		img = image;
+		UERROR("Image format must be mono8. Current has %d channels and type = %d, size=%d,%d",
+				image.channels(), image.type(), image.cols, image.rows);
 	}
 
-	cv::Mat imgRoi(img, roi);
+	return keypoints;
-	if(_gpuVersion && cv::gpu::getCudaEnabledDeviceCount())
+}
+
+cv::Mat Feature2D::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	return generateDescriptorsImpl(image, keypoints);
+}
+
+//////////////////////////
+//SURF
+//////////////////////////
+SURF::SURF(const ParametersMap & parameters) :
+		_surf(0),
+		_gpuSurf(0)
+{
+	double hessianThreshold = Parameters::defaultSURFHessianThreshold();
+	int nOctaves = Parameters::defaultSURFOctaves();
+	int nOctaveLayers = Parameters::defaultSURFOctaveLayers();
+	bool extended = Parameters::defaultSURFExtended();
+	bool upright = Parameters::defaultSURFUpright();
+	float gpuKeypointsRatio = Parameters::defaultSURFGpuKeypointsRatio();
+	bool gpuVersion = Parameters::defaultSURFGpuVersion();
+
+	Parameters::parse(parameters, Parameters::kSURFExtended(), extended);
+	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), hessianThreshold);
+	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), nOctaveLayers);
+	Parameters::parse(parameters, Parameters::kSURFOctaves(), nOctaves);
+	Parameters::parse(parameters, Parameters::kSURFUpright(), upright);
+	Parameters::parse(parameters, Parameters::kSURFGpuKeypointsRatio(), gpuKeypointsRatio);
+	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), gpuVersion);
+
+	if(gpuVersion && cv::gpu::getCudaEnabledDeviceCount())
 	{
-		cv::gpu::GpuMat imgGpu(imgRoi);
+		_gpuSurf = new cv::gpu::SURF_GPU(hessianThreshold, nOctaves, nOctaveLayers, extended, gpuKeypointsRatio, upright);
-		cv::gpu::GpuMat keypointsGpu;
-		cv::gpu::SURF_GPU surfGpu(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, 0.01f, _upright);
-		surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
-		surfGpu.downloadKeypoints(keypointsGpu, keypoints);
 	}
 	else
 	{
-		if(_gpuVersion)
+		if(gpuVersion)
 		{
 			UWARN("GPU version of SURF not available! Using CPU version instead...");
 		}
-		ULOGGER_DEBUG("%f %d %d %d %d", _hessianThreshold, _nOctaves, _nOctaveLayers, _extended?1:0, _upright?1:0);
+
-		cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
+		_surf = new cv::SURF (hessianThreshold, nOctaves, nOctaveLayers, extended, upright);
-		detector.detect(imgRoi, keypoints);
 	}
-
-	ULOGGER_DEBUG("");
-	return keypoints;
 }
 
-//////////////////////////
+SURF::~SURF()
-//SIFTDetector
-//////////////////////////
-SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
-		KeypointDetector(parameters),
-		_nfeatures(Parameters::defaultSIFTNFeatures()),
-		_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
-		_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
-		_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
-		_sigma(Parameters::defaultSIFTSigma())
 {
-	this->parseParameters(parameters);
+	if(_surf)
+	{
+		delete _surf;
+	}
+	if(_gpuSurf)
+	{
+		delete _gpuSurf;
+	}
 }
 
-SIFTDetector::~SIFTDetector()
+std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
 {
-}
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-
-void SIFTDetector::parseParameters(const ParametersMap & parameters)
-{
-	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), _contrastThreshold);
-	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), _edgeThreshold);
-	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), _nfeatures);
-	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), _nOctaveLayers);
-	Parameters::parse(parameters, Parameters::kSIFTSigma(), _sigma);
-	KeypointDetector::parseParameters(parameters);
-}
-
-std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
-{
-	ULOGGER_DEBUG("");
 	std::vector<cv::KeyPoint> keypoints;
-	if(image.empty())
+	cv::Mat imgRoi(image, roi);
+	if(_gpuSurf)
 	{
-		ULOGGER_ERROR("Image is null ?!?");
+		cv::gpu::GpuMat imgGpu(imgRoi);
-		return keypoints;
+		(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints);
-	}
-	// SURF support only grayscale images
-	cv::Mat imageGrayScale;
-	if(image.channels() != 1 || image.depth() != CV_8U)
-	{
-		cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
-	}
-	cv::Mat img;
-	if(!imageGrayScale.empty())
-	{
-		img = imageGrayScale;
 	}
 	else
 	{
-		img = image;
+		_surf->detect(imgRoi, keypoints);
 	}
 
-	cv::Mat imgRoi(img, roi);
-	cv::SIFT detector(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
-	detector.detect(imgRoi, keypoints); // Opencv surf keypoints
 	return keypoints;
 }
 
+cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+	if(_gpuSurf)
+	{
+		cv::gpu::GpuMat imgGpu(image);
+		cv::gpu::GpuMat descriptorsGPU;
+		(*_gpuSurf)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU, true);
+
+		// Download descriptors
+		if (descriptorsGPU.empty())
+			descriptors = cv::Mat();
+		else
+		{
+			UASSERT(descriptorsGPU.type() == CV_32F);
+			descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
+			descriptorsGPU.download(descriptors);
+		}
+	}
+	else
+	{
+		_surf->compute(image, keypoints, descriptors);
+	}
+
+	return descriptors;
+}
+
+//////////////////////////
+//SIFT
+//////////////////////////
+SIFT::SIFT(const ParametersMap & parameters) :
+	_sift(0)
+{
+	int nfeatures = Parameters::defaultSIFTNFeatures();
+	int nOctaveLayers = Parameters::defaultSIFTNOctaveLayers();
+	double contrastThreshold = Parameters::defaultSIFTContrastThreshold();
+	double edgeThreshold = Parameters::defaultSIFTEdgeThreshold();
+	double sigma = Parameters::defaultSIFTSigma();
+
+	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold);
+	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold);
+	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures);
+	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers);
+	Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma);
+
+	_sift = new cv::SIFT(nfeatures, nOctaveLayers, contrastThreshold, edgeThreshold, sigma);
+}
+
+SIFT::~SIFT()
+{
+	if(_sift)
+	{
+		delete _sift;
+	}
+}
+
+std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat imgRoi(image, roi);
+	_sift->detect(imgRoi, keypoints); // Opencv keypoints
+	return keypoints;
+}
+
+cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+	_sift->compute(image, keypoints, descriptors);
+	return descriptors;
+}
+
+//////////////////////////
+//ORB
+//////////////////////////
+ORB::ORB(const ParametersMap & parameters) :
+		_orb(0),
+		_gpuOrb(0)
+{
+	int nFeatures = Parameters::defaultORBNFeatures();
+	float scaleFactor = Parameters::defaultORBScaleFactor();
+	int nLevels = Parameters::defaultORBNLevels();
+	int edgeThreshold = Parameters::defaultORBEdgeThreshold();
+	int firstLevel = Parameters::defaultORBFirstLevel();
+	int WTA_K = Parameters::defaultORBWTA_K();
+	int scoreType = Parameters::defaultORBScoreType();
+	int patchSize = Parameters::defaultORBPatchSize();
+	bool gpu = Parameters::defaultORBGpu();
+
+	int fastThreshold = Parameters::defaultFASTThreshold();
+	bool nonmaxSuppresion = Parameters::defaultFASTNonmaxSuppression();
+
+	Parameters::parse(parameters, Parameters::kORBNFeatures(), nFeatures);
+	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor);
+	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels);
+	Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold);
+	Parameters::parse(parameters, Parameters::kORBFirstLevel(), firstLevel);
+	Parameters::parse(parameters, Parameters::kORBWTA_K(), WTA_K);
+	Parameters::parse(parameters, Parameters::kORBScoreType(), scoreType);
+	Parameters::parse(parameters, Parameters::kORBPatchSize(), patchSize);
+	Parameters::parse(parameters, Parameters::kORBGpu(), gpu);
+
+	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold);
+	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppresion);
+
+	if(gpu && cv::gpu::getCudaEnabledDeviceCount())
+	{
+		_gpuOrb = new cv::gpu::ORB_GPU(nFeatures, scaleFactor, nLevels, edgeThreshold, firstLevel, WTA_K, scoreType, patchSize);
+		_gpuOrb->setFastParams(fastThreshold, nonmaxSuppresion);
+	}
+	else
+	{
+		if(gpu)
+		{
+			UWARN("GPU version of ORB not available! Using CPU version instead...");
+		}
+		_orb = new cv::ORB(nFeatures, scaleFactor, nLevels, edgeThreshold, firstLevel, WTA_K, scoreType, patchSize);
+	}
+}
+
+ORB::~ORB()
+{
+	if(_orb)
+	{
+		delete _orb;
+	}
+	if(_gpuOrb)
+	{
+		delete _gpuOrb;
+	}
+}
+
+std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat imgRoi(image, roi);
+	if(_gpuOrb)
+	{
+		cv::gpu::GpuMat imgGpu(imgRoi);
+		(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints);
+	}
+	else
+	{
+		_orb->detect(imgRoi, keypoints);
+	}
+
+	return keypoints;
+}
+
+cv::Mat ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+	if(image.empty())
+	{
+		ULOGGER_ERROR("Image is null ?!?");
+		return descriptors;
+	}
+	if(_gpuOrb)
+	{
+		cv::gpu::GpuMat imgGpu(image);
+		cv::gpu::GpuMat descriptorsGPU;
+		(*_gpuOrb)(imgGpu, cv::gpu::GpuMat(), keypoints, descriptorsGPU);
+
+		// Download descriptors
+		if (descriptorsGPU.empty())
+			descriptors = cv::Mat();
+		else
+		{
+			UASSERT(descriptorsGPU.type() == CV_32F);
+			descriptors = cv::Mat(descriptorsGPU.size(), CV_32F);
+			descriptorsGPU.download(descriptors);
+		}
+	}
+	else
+	{
+		_orb->compute(image, keypoints, descriptors);
+	}
+
+	return descriptors;
+}
+
+//////////////////////////
+//FAST
+//////////////////////////
+FAST::FAST(const ParametersMap & parameters) :
+		_fast(0),
+		_gpuFast(0)
+{
+	int threshold = Parameters::defaultFASTThreshold();
+	bool nonmaxSuppression = Parameters::defaultFASTNonmaxSuppression();
+	bool gpu = Parameters::defaultFASTGpu();
+	double gpuKeypointsRatio = Parameters::defaultFASTGpuKeypointsRatio();
+
+	Parameters::parse(parameters, Parameters::kFASTThreshold(), threshold);
+	Parameters::parse(parameters, Parameters::kFASTNonmaxSuppression(), nonmaxSuppression);
+	Parameters::parse(parameters, Parameters::kFASTGpu(), gpu);
+	Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio);
+
+	if(gpu && cv::gpu::getCudaEnabledDeviceCount())
+	{
+		_gpuFast = new cv::gpu::FAST_GPU(threshold, nonmaxSuppression, gpuKeypointsRatio);
+	}
+	else
+	{
+		if(gpu)
+		{
+			UWARN("GPU version of FAST not available! Using CPU version instead...");
+		}
+		_fast = new cv::FastFeatureDetector(threshold, nonmaxSuppression);
+	}
+}
+
+FAST::~FAST()
+{
+	if(_fast)
+	{
+		delete _fast;
+	}
+	if(_gpuFast)
+	{
+		delete _gpuFast;
+	}
+}
+
+std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat imgRoi(image, roi);
+	if(_gpuFast)
+	{
+		cv::gpu::GpuMat imgGpu(imgRoi);
+		(*_gpuFast)(imgGpu, cv::gpu::GpuMat(), keypoints);
+	}
+	else
+	{
+		_fast->detect(imgRoi, keypoints); // Opencv keypoints
+	}
+	return keypoints;
+}
+
+//////////////////////////
+//FAST-BRIEF
+//////////////////////////
+FAST_BRIEF::FAST_BRIEF(const ParametersMap & parameters) :
+	FAST(parameters),
+	_brief(0)
+{
+	int bytes = Parameters::defaultBRIEFBytes();
+	Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes);
+	_brief = new cv::BriefDescriptorExtractor(bytes);
+}
+
+FAST_BRIEF::~FAST_BRIEF()
+{
+	if(_brief)
+	{
+		delete _brief;
+	}
+}
+
+cv::Mat FAST_BRIEF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+	_brief->compute(image, keypoints, descriptors);
+	return descriptors;
+}
+
+//////////////////////////
+//FAST-FREAK
+//////////////////////////
+FAST_FREAK::FAST_FREAK(const ParametersMap & parameters) :
+	FAST(parameters),
+	_freak(0)
+{
+	bool orientationNormalized = Parameters::defaultFREAKOrientationNormalized();
+	bool scaleNormalized = Parameters::defaultFREAKScaleNormalized();
+	float patternScale = Parameters::defaultFREAKPatternScale();
+	int nOctaves = Parameters::defaultFREAKNOctaves();
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves);
+
+	_freak = new cv::FREAK(orientationNormalized, scaleNormalized, patternScale, nOctaves);
+}
+
+FAST_FREAK::~FAST_FREAK()
+{
+	if(_freak)
+	{
+		delete _freak;
+	}
+}
+
+cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat descriptors;
+	_freak->compute(image, keypoints, descriptors);
+	return descriptors;
+}
+
 }

corelib/src/Memory.cpp

@@ -63,8 +63,8 @@ Memory::Memory(const ParametersMap & parameters) :
 	_memoryChanged(false),
 	_signaturesAdded(0),
 
-	_keypointDetector(0),
+	_feature2D(0),
-	_keypointDescriptor(0),
+	_featureType(Feature2D::kFeatureUndef),
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
 	_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
 	_parallelized(Parameters::defaultKpParallelized()),
@@ -152,7 +152,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 				if(!((*iter)->isBadSignature() && _badSignaturesIgnored))
 				{
 					_signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
-					if((int)_stMem.size() <= _maxStMemSize)
+					if(_maxStMemSize == 0 || (int)_stMem.size() <= _maxStMemSize)
 					{
 						_stMem.insert((*iter)->id());
 					}
@@ -266,13 +266,9 @@ Memory::~Memory()
 		}
 	}
 
-	if(_keypointDetector)
+	if(_feature2D)
 	{
-		delete _keypointDetector;
+		delete _feature2D;
-	}
-	if(_keypointDescriptor)
-	{
-		delete _keypointDescriptor;
 	}
 	if(_vwd)
 	{
@@ -294,7 +290,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
 	Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
 
-	UASSERT_MSG(_maxStMemSize > 0, uFormat("value=%d", _maxStMemSize).c_str());
+	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
 	UASSERT_MSG(_recentWmRatio >= 0.0f && _recentWmRatio <= 1.0f, uFormat("value=%f", _recentWmRatio).c_str());
 
@@ -368,48 +364,45 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	}
 
 	//Keypoint detector
-	KeypointDetector::DetectorType detectorStrategy = KeypointDetector::kDetectorUndef;
+	Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
 	if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
 	{
-		detectorStrategy = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
+		detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
 	}
-	if(!_keypointDetector ||  detectorStrategy!=KeypointDetector::kDetectorUndef)
+	if(detectorStrategy!=Feature2D::kFeatureUndef)
 	{
 		UDEBUG("new detector strategy %d", int(detectorStrategy));
-		if(_keypointDetector)
+		if(_feature2D)
 		{
-			delete _keypointDetector;
+			delete _feature2D;
-			_keypointDetector = 0;
+			_feature2D = 0;
-		}
+			_featureType = Feature2D::kFeatureUndef;
-		if(_keypointDescriptor)
-		{
-			delete _keypointDescriptor;
-			_keypointDescriptor = 0;
 		}
 		switch(detectorStrategy)
 		{
-		case KeypointDetector::kDetectorSift:
+		case Feature2D::kFeatureSift:
-			_keypointDetector = new SIFTDetector(parameters);
+			_feature2D = new SIFT(parameters);
-			_keypointDescriptor = new SIFTDescriptor(parameters);
+			_featureType = Feature2D::kFeatureSift;
 			break;
-		case KeypointDetector::kDetectorSurf:
+		case Feature2D::kFeatureFastBrief:
+			_feature2D = new FAST_BRIEF(parameters);
+			_featureType = Feature2D::kFeatureFastBrief;
+			break;
+		case Feature2D::kFeatureFastFreak:
+			_feature2D = new FAST_FREAK(parameters);
+			_featureType = Feature2D::kFeatureFastFreak;
+			break;
+		case Feature2D::kFeatureOrb:
+			_feature2D = new ORB(parameters);
+			_featureType = Feature2D::kFeatureOrb;
+			break;
+		case Feature2D::kFeatureSurf:
 		default:
-			_keypointDetector = new SURFDetector(parameters);
+			_feature2D = new SURF(parameters);
-			_keypointDescriptor = new SURFDescriptor(parameters);
+			_featureType = Feature2D::kFeatureSurf;
 			break;
 		}
 	}
-	else
-	{
-		if(_keypointDetector)
-		{
-			_keypointDetector->parseParameters(parameters);
-		}
-		if(_keypointDescriptor)
-		{
-			_keypointDescriptor->parseParameters(parameters);
-		}
-	}
 }
 
 void Memory::preUpdate()
@@ -487,7 +480,7 @@ bool Memory::update(const Image & image, Statistics * stats)
 	//============================================================
 	// Transfer the oldest signature of the short-term memory to the working memory
 	//============================================================
-	while(_stMem.size() && (int)_stMem.size() > _maxStMemSize)
+	while(_stMem.size() && _maxStMemSize>0 && (int)_stMem.size() > _maxStMemSize)
 	{
 		UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
 		_workingMem.insert(_workingMem.end(), *_stMem.begin());
@@ -616,9 +609,9 @@ Signature * Memory::_getSignature(int id) const
 	return uValue(_signatures, id, (Signature*)0);
 }
 
-int Memory::getVWDictionarySize() const
+const VWDictionary * Memory::getVWDictionary() const
 {
-	return _vwd->getVisualWords().size();
+	return _vwd;
 }
 
 void Memory::getPose(int locationId, Transform & pose, bool lookInDatabase) const
@@ -1436,7 +1429,10 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
 	return removableSignatures;
 }
 
-void Memory::moveToTrash(Signature * s, bool saveToDatabase)
+/**
+ * If saveToDatabase=false, deleted words are filled in deletedWords.
+ */
+void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * deletedWords)
 {
 	UDEBUG("id=%d", s?s->id():0);
 	if(s)
@@ -1453,9 +1449,10 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
 				// neighbor to s
 				if(n)
 				{
-					if(iter->first > s->id() && (n->getNeighbors().size() > 1 || !n->hasNeighbor(s->id())))
+					if(iter->first > s->id() && (n->getNeighbors().size() > 2 || !n->hasNeighbor(s->id())))
 					{
-						UWARN("Neighbor %d of %d is newer, removing neighbor link may split the map!", iter->first, s->id());
+						UWARN("Neighbor %d of %d is newer, removing neighbor link may split the map!",
+								iter->first, s->id());
 					}
 
 					n->removeNeighbor(s->id());
@@ -1503,7 +1500,27 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase)
 			s->setWeight(0);
 		}
 
-		this->disableWordsRef(s->id(), saveToDatabase);
+		this->disableWordsRef(s->id());
+		if(!saveToDatabase)
+		{
+			std::list<int> keys = uUniqueKeys(s->getWords());
+			for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
+			{
+				// assume just removed word doesn't have any other references
+				VisualWord * w = _vwd->getUnusedWord(*i);
+				if(w)
+				{
+					std::vector<VisualWord*> wordToDelete;
+					wordToDelete.push_back(w);
+					_vwd->removeWords(wordToDelete);
+					if(deletedWords)
+					{
+						deletedWords->push_back(w->id());
+					}
+					delete w;
+				}
+			}
+		}
 
 		_workingMem.erase(s->id());
 		_stMem.erase(s->id());
@@ -1542,13 +1559,13 @@ const Signature * Memory::getLastWorkingSignature() const
 	return _lastSignature;
 }
 
-void Memory::deleteLocation(int locationId)
+void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
 {
-	UINFO("Deleting location %d", locationId);
+	UDEBUG("Deleting location %d", locationId);
 	Signature * location = _getSignature(locationId);
 	if(location)
 	{
-		this->moveToTrash(location, false);
+		this->moveToTrash(location, false, deletedWords);
 	}
 }
 
@@ -1640,12 +1657,12 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 			}
 			else if(inliersCount < _bowMinInliers)
 			{
-				UINFO("Not enough inliers %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
+				UINFO("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
 			}
 		}
 		else
 		{
-			UDEBUG("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
+			UINFO("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
 		}
 	}
 	else if(!oldS.isBadSignature() && !newS.isBadSignature())
@@ -2783,10 +2800,10 @@ void Memory::extractKeypointsAndDescriptors(
 	if(_wordsPerImageTarget >= 0)
 	{
 		UTimer timer;
-		if(_keypointDetector)
+		if(_feature2D)
 		{
-			cv::Rect roi = KeypointDetector::computeRoi(image, _roiRatios);
+			cv::Rect roi = computeRoi(image, _roiRatios);
-			keypoints = _keypointDetector->generateKeypoints(image, 0, roi);
+			keypoints = _feature2D->generateKeypoints(image, 0, roi);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
 
 			filterKeypointsByDepth(keypoints, depth, depthConstant, _wordsMaxDepth);
@@ -2795,7 +2812,7 @@ void Memory::extractKeypointsAndDescriptors(
 
 		if(keypoints.size())
 		{
-			descriptors = _keypointDescriptor->generateDescriptors(image, keypoints);
+			descriptors = _feature2D->generateDescriptors(image, keypoints);
 			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
 		}
 	}
@@ -2876,12 +2893,12 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 		preUpdateThread.start();
 	}
 
-	if(!image.descriptors().empty())
+	if(!image.descriptors().empty() && image.featureType() == _featureType)
 	{
 		// DESCRIPTORS
 		if(image.descriptors().rows && image.descriptors().rows >= _badSignRatio * float(meanWordsPerLocation))
 		{
-			UASSERT(image.descriptors().type() == CV_32F);
+			UASSERT(image.descriptors().type() == CV_32F || image.descriptors().type() == CV_8U);
 			descriptors = image.descriptors();
 			keypoints = image.keypoints();
 		}
@@ -2891,7 +2908,18 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 	else
 	{
 		// IMAGE RAW
-		this->extractKeypointsAndDescriptors(image.image(), image.depth(), image.depthConstant(), keypoints, descriptors);
+		cv::Mat imageMono;
+		// convert to grayscale
+		if(image.image().channels() > 1)
+		{
+			cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
+		}
+		else
+		{
+			imageMono = image.image();
+		}
+
+		this->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
 
 		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
@@ -2999,11 +3027,11 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 	return s;
 }
 
-void Memory::disableWordsRef(int signatureId, bool saveToDatabase)
+void Memory::disableWordsRef(int signatureId)
 {
 	UDEBUG("id=%d", signatureId);
 
-	Signature * ss = dynamic_cast<Signature *>(this->_getSignature(signatureId));
+	Signature * ss = this->_getSignature(signatureId);
 	if(ss && ss->isEnabled())
 	{
 		const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
@@ -3013,18 +3041,6 @@ void Memory::disableWordsRef(int signatureId, bool saveToDatabase)
 		for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
 		{
 			_vwd->removeAllWordRef(*i, signatureId);
-			if(!saveToDatabase)
-			{
-				// assume just removed word doesn't have any other references
-				VisualWord * w = _vwd->getUnusedWord(*i);
-				if(w)
-				{
-					std::vector<VisualWord*> wordToDelete;
-					wordToDelete.push_back(w);
-					_vwd->removeWords(wordToDelete);
-					delete w;
-				}
-			}
 		}
 
 		count -= _vwd->getTotalActiveReferences();
@@ -3103,8 +3119,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 	if(vws.size())
 	{
 		//Search in the dictionary
-		bool reactivatedWordsComparedToNewWords = true;
+		std::vector<int> vwActiveIds = _vwd->findNN(vws);
-		std::vector<int> vwActiveIds = _vwd->findNN(vws, reactivatedWordsComparedToNewWords);
 		UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks());
 		int i=0;
 		for(std::list<VisualWord *>::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws)

corelib/src/NearestNeighbor.cpp

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

corelib/src/NearestNeighbor.h

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

corelib/src/Odometry.cpp

@@ -18,11 +18,14 @@
 #include <rtabmap/core/Features2d.h>
 
 #include <rtabmap/core/Memory.h>
+#include <rtabmap/core/VWDictionary.h>
 #include "rtabmap/core/Signature.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/transforms.h>
 
+#include <opencv2/gpu/gpu.hpp>
+
 #if _MSC_VER
 	#define ISFINITE(value) _finite(value)
 #else
@@ -31,31 +34,6 @@
 
 namespace rtabmap {
 
-Odometry::Odometry(
-		float inlierDistance,
-		int maxWords,
-		int minInliers,
-		int iterations,
-		float wordsRatio,
-		float maxDepth,
-		float linearUpdate,
-		float angularUpdate,
-		int resetCoutdown) :
-			_maxFeatures(maxWords),
-			_minInliers(minInliers),
-			_inlierDistance(inlierDistance),
-			_iterations(iterations),
-			_wordsRatio(wordsRatio),
-			_maxDepth(maxDepth),
-			_linearUpdate(linearUpdate),
-			_angularUpdate(angularUpdate),
-			_resetCountdown(resetCoutdown),
-			_pose(Transform::getIdentity()),
-			_resetCurrentCount(0)
-{
-
-}
-
 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_maxFeatures(Parameters::defaultOdomMaxWords()),
 		_minInliers(Parameters::defaultOdomMinInliers()),
@@ -66,6 +44,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
 		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
+		_localHistory(Parameters::defaultOdomLocalHistory()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0)
 {
@@ -78,6 +57,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomWordsRatio(), _wordsRatio);
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
+	Parameters::parse(parameters, Parameters::kOdomLocalHistory(), _localHistory);
 }
 
 void Odometry::reset()
@@ -116,57 +96,71 @@ Transform Odometry::process(Image & image, int * quality)
 	}
 	return Transform();
 }
-OdometryBinary::OdometryBinary(
-		float inlierDistance,
-		int maxWords,
-		int minInliers,
-		int iterations,
-		float wordsRatio,
-		float maxDepth,
-		float linearUpdate,
-		float angularUpdate,
-		int resetCoutdown,
-		int briefBytes,
-		int fastThreshold,
-		bool fastNonmaxSuppression,
-		bool bruteForceMatching) :
-	Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
- 	_briefBytes(briefBytes),
-	_fastThreshold(fastThreshold),
-	_fastNonmaxSuppression(fastNonmaxSuppression),
-	_bruteForceMatching(bruteForceMatching)
-{
 
-}
+//OdometryBOW
-
+OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
-OdometryBinary::OdometryBinary(const ParametersMap & parameters) :
 	Odometry(parameters),
- 	_briefBytes(Parameters::defaultOdomBinBriefBytes()),
+	_memory(0)
-	_fastThreshold(Parameters::defaultOdomBinFastThreshold()),
-	_fastNonmaxSuppression(Parameters::defaultOdomBinFastNonmaxSuppression()),
-	_bruteForceMatching(Parameters::defaultOdomBinBruteForceMatching())
 {
-	Parameters::parse(parameters, Parameters::kOdomBinBriefBytes(), _briefBytes);
+	ParametersMap customParameters;
-	Parameters::parse(parameters, Parameters::kOdomBinFastThreshold(), _fastThreshold);
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
-	Parameters::parse(parameters, Parameters::kOdomBinFastNonmaxSuppression(), _fastNonmaxSuppression);
+	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
-	Parameters::parse(parameters, Parameters::kOdomBinBruteForceMatching(), _bruteForceMatching);
+	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+	int nn = Parameters::defaultOdomNearestNeighbor();
+	float nndr = Parameters::defaultOdomNNDR();
+	int odomType = Parameters::defaultOdomType();
+	Parameters::parse(parameters, Parameters::kOdomNearestNeighbor(), nn);
+	Parameters::parse(parameters, Parameters::kOdomNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kOdomType(), odomType);
+	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
+	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
+	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(odomType)));
+
+	// add only feature stuff
+	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FREAK") == 0)
+		{
+			customParameters.insert(*iter);
+		}
+	}
+
+	_memory = new Memory(customParameters);
+	if(!_memory->init("", false, ParametersMap(), false))
+	{
+		UERROR("Error initializing the memory for BOW Odometry.");
+	}
 }
 
-void OdometryBinary::reset()
+OdometryBOW::~OdometryBOW()
+{
+	delete _memory;
+}
+
+
+void OdometryBOW::reset()
 {
 	Odometry::reset();
-	_lastKeypoints.clear();
+	_memory->init("", false, ParametersMap(), false);
-	_lastDescriptors = cv::Mat();
+	localMap_.clear();
-	_lastDepth = cv::Mat();
 }
 
 
 // return not null transform if odometry is correctly computed
-Transform OdometryBinary::computeTransform(Image & image, int * quality)
+Transform OdometryBOW::computeTransform(Image & image, int * quality)
 {
 	UTimer timer;
-	cv::Mat imageMono;
 	Transform output;
+
+	cv::Mat imageMono;
 	// convert to grayscale
 	if(image.image().channels() > 1)
 	{
@@ -177,274 +171,19 @@ Transform OdometryBinary::computeTransform(Image & image, int * quality)
 		imageMono = image.image();
 	}
 
-	cv::FastFeatureDetector detector(_fastThreshold, _fastNonmaxSuppression);
-	std::vector<cv::KeyPoint> newKeypoints;
-	detector.detect(imageMono, newKeypoints);
-
-	limitKeypoints(newKeypoints, this->getMaxFeatures());
-
-	cv::BriefDescriptorExtractor extractor(_briefBytes);
-	cv::Mat newDescriptors;
-	extractor.compute(imageMono, newKeypoints, newDescriptors);
-
-	int inliers = 0;
-	int correspondences = 0;
-
-	if(_lastKeypoints.size())
-	{
-		if(newDescriptors.rows && newDescriptors.rows > (int)(getWordsRatio() * float(_lastKeypoints.size()))) // at least 50% keypoints
-		{
-			cv::Mat results;
-			cv::Mat dists;
-			int k=1; // find the 1 nearest neighbor
-			std::vector<std::vector<cv::DMatch> > matches;
-
-			if(_bruteForceMatching)
-			{
-				cv::BFMatcher matcher(cv::NORM_HAMMING);
-				  matcher.knnMatch(newDescriptors, _lastDescriptors, matches, k);
-			}
-			else
-			{
-				 // Create Flann LSH index
-				cv::flann::Index flannIndex(_lastDescriptors, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
-				results = cv::Mat(newDescriptors.rows, k, CV_32SC1);
-				dists = cv::Mat(newDescriptors.rows, k, CV_32FC1);
-
-				// search (nearest neighbor)
-				flannIndex.knnSearch(newDescriptors, results, dists, k, cv::flann::SearchParams() );
-			}
-
-			pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_1(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_2(new pcl::PointCloud<pcl::PointXYZ>);
-			std::vector<int> indexes_1, indexes_2;
-			std::vector<uchar> outlier_mask;
-			// Check if this descriptor matches with those of the objects
-			mpts_1->resize(newDescriptors.rows);
-			mpts_2->resize(newDescriptors.rows);
-			UDEBUG("newDescriptors=%d _lastKeypoints=%d time=%fs", newDescriptors.rows, _lastKeypoints.size(), timer.elapsed());
-			int oi = 0;
-			if(_bruteForceMatching)
-			{
-				for(unsigned int i=0; i<matches.size(); ++i)
-				{
-					pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
-							int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.x+0.5f),
-							int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.y+0.5f),
-							(float)imageMono.cols/2,
-							(float)imageMono.rows/2,
-							1.0f/image.depthConstant(),
-							1.0f/image.depthConstant());
-					if(matches.at(i).at(0).trainIdx >=0)
-					{
-						pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
-								int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.x+0.5f),
-								int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.y+0.5f),
-								(float)imageMono.cols/2,
-								(float)imageMono.rows/2,
-								1.0f/image.depthConstant(),
-								1.0f/image.depthConstant());
-
-						if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
-						   (this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
-						{
-							mpts_1->at(oi) = pt1;
-							mpts_2->at(oi) = pt2;
-							++oi;
-						}
-					}
-					else
-					{
-						UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
-					}
-				}
-			}
-			else
-			{
-				for(int i=0; i<newDescriptors.rows; ++i)
-				{
-					pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
-							int(newKeypoints.at(i).pt.x+0.5f),
-							int(newKeypoints.at(i).pt.y+0.5f),
-							(float)imageMono.cols/2,
-							(float)imageMono.rows/2,
-							1.0f/image.depthConstant(),
-							1.0f/image.depthConstant());
-					if(results.at<int>(i,0) >=0)
-					{
-						pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
-								int(_lastKeypoints.at(results.at<int>(i,0)).pt.x+0.5f),
-								int(_lastKeypoints.at(results.at<int>(i,0)).pt.y+0.5f),
-								(float)imageMono.cols/2,
-								(float)imageMono.rows/2,
-								1.0f/image.depthConstant(),
-								1.0f/image.depthConstant());
-
-						if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
-						   (this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
-						{
-							mpts_1->at(oi) = pt1;
-							mpts_2->at(oi) = pt2;
-							++oi;
-						}
-					}
-					else
-					{
-						UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
-					}
-				}
-			}
-			mpts_1->resize(oi);
-			mpts_2->resize(oi);
-
-			UDEBUG("Correspondences = %d", oi);
-
-			if(oi >= this->getMinInliers())
-			{
-				mpts_1 = util3d::transformPointCloud(mpts_1, image.localTransform()); // new
-				mpts_2 = util3d::transformPointCloud(mpts_2, image.localTransform()); // previous
-				correspondences = mpts_2->size();
-				Transform t = util3d::transformFromXYZCorrespondences(
-						mpts_1,
-						mpts_2,
-						this->getInlierDistance(),
-						this->getIterations(),
-						&inliers);
-				float x,y,z, roll,pitch,yaw;
-				pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(t), x,y,z, roll,pitch,yaw);
-
-				if(quality)
-				{
-					*quality = inliers;
-				}
-
-				// Large transforms may be erroneous computed transforms, so keep under 1 m
-				if(inliers >= this->getMinInliers())
-				{
-					if(isLargeEnoughTransform(t))
-					{
-						_lastKeypoints = newKeypoints;
-						_lastDescriptors = newDescriptors;
-						_lastDepth = image.depth().clone();
-						output = t;
-					}
-					else
-					{
-						output.setIdentity();
-					}
-				}
-				else
-				{
-					UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
-				}
-			}
-			else
-			{
-				UWARN("Not enough inliers %d < %d", oi, this->getMinInliers());
-			}
-		}
-		else if(newDescriptors.rows)
-		{
-			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-					getWordsRatio()*100.0f, newDescriptors.rows, _lastKeypoints.size());
-		}
-		else
-		{
-			UWARN("No feature extracted!");
-		}
-	}
-	else
-	{
-		_lastKeypoints = newKeypoints;
-		_lastDescriptors = newDescriptors;
-		_lastDepth = image.depth().clone();
-		output.setIdentity();
-	}
-
-	UINFO("Odom update time = %fs features=%d inliers=%d/%d",
-			timer.elapsed(),
-			newDescriptors.rows,
-			inliers,
-			correspondences);
-
-	return output;
-}
-
-//OdometryBOW
-
-OdometryBOW::OdometryBOW(
-		int detectorType, // SURF or SIFT
-		float inlierDistance,
-		int maxWords,
-		int minInliers,
-		int iterations,
-		float wordsRatio,
-		float maxDepth,
-		float linearUpdate,
-		float angularUpdate,
-		int resetCoutdown,
-		float surfHessianThreshold,
-		float nndr) : // nearest neighbor distance ratio
-	Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
-	_memory(new Memory())
-{
-	ParametersMap customParameters;
-	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxWords)));
-	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
-	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(detectorType)));
-	customParameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(surfHessianThreshold)));
-	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
-	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
-	if(!_memory->init("", false, customParameters, false))
-	{
-		UERROR("Error initializing the memory for BOW Odometry.");
-	}
-}
-
-OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
-	Odometry(parameters),
-	_memory(new Memory(parameters))
-{
-	ParametersMap customParameters;
-	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
-	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
-	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
-	if(!_memory->init("", false, customParameters, false))
-	{
-		UERROR("Error initializing the memory for BOW Odometry.");
-	}
-}
-
-OdometryBOW::~OdometryBOW()
-{
-	UDEBUG("");
-	delete _memory;
-	UDEBUG("");
-}
-
-
-void OdometryBOW::reset()
-{
-	Odometry::reset();
-	_memory->init("", false, ParametersMap(), false);
-}
-
-
-// return not null transform if odometry is correctly computed
-Transform OdometryBOW::computeTransform(Image & image, int * quality)
-{
-	UTimer timer;
-	Transform output;
-
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
-	_memory->extractKeypointsAndDescriptors(image.image(), image.depth(), image.depthConstant(), keypoints, descriptors);
+	_memory->extractKeypointsAndDescriptors(imageMono, image.depth(), image.depthConstant(), keypoints, descriptors);
 
-	image.setDescriptors(descriptors);
+	image.setDescriptors(descriptors, _memory->getFeatureType());
 	image.setKeypoints(keypoints);
 
+	if(this->getLocalHistory() && this->getLocalHistory() < descriptors.rows)
+	{
+		UWARN("Local history words size (%d) is smaller than extracted features from the current frame (%d).",
+				this->getLocalHistory(), descriptors.rows);
+	}
+
 	int inliers = 0;
 	int correspondences = 0;
 
@@ -455,22 +194,24 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 		if(previousSignature && newSignature)
 		{
 			Transform transform;
+			std::set<int> uniqueCorrespondences;
 			if(newSignature->getWords3().size() < (unsigned int)(getWordsRatio() * float(previousSignature->getWords3().size())))
 			{
 				UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
 						getWordsRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
 			}
-			else if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
+			else if(!localMap_.empty() && !newSignature->getWords3().empty())
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
 
 				util3d::findCorrespondences(
-						previousSignature->getWords3(),
+						localMap_,
 						newSignature->getWords3(),
 						*inliers1,
 						*inliers2,
-						this->getMaxDepth());
+						this->getMaxDepth(),
+						&uniqueCorrespondences);
 
 				if((int)inliers1->size() >= this->getMinInliers())
 				{
@@ -493,10 +234,6 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 						transform.setNull();
 						UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
 					}
-					//else if(!transform.isNull() && true)
-					//{
-					//	transform = _memory->computeIcpTransform(*newSignature, *previousSignature, transform, true);
-					//}
 				}
 				else
 				{
@@ -508,46 +245,121 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
 			{
 				_memory->deleteLocation(newSignature->id());
 			}
-			else if(!isLargeEnoughTransform(transform))
-			{
-				output.setIdentity();
-				_memory->deleteLocation(newSignature->id());
-			}
 			else
 			{
-				output = transform;
+				if(this->getLocalHistory()<=0)
-				_memory->deleteLocation(previousSignature->id());
+				{
+					output = this->getPose().inverse() * transform; // make it incremental
+					if(!isLargeEnoughTransform(transform))
+					{
+						// Transform not large enough, keep the old signature
+						_memory->deleteLocation(newSignature->id());
+					}
+					else
+					{
+						_memory->deleteLocation(previousSignature->id());
+						localMap_.clear();
+						// update local map
+						std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+						for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+						{
+							if(newSignature->getWords3().count(*iter) == 1)
+							{
+								const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+								if(pcl::isFinite(pt) &&
+								   (pt.x != 0 || pt.y != 0 || pt.z != 0) &&
+								   (this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
+								{
+									pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
+									localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
+								}
+							}
+						}
+					}
+				}
+				else
+				{
+					output = this->getPose().inverse() * transform; // make it incremental
+
+					if(isLargeEnoughTransform(transform))
+					{
+						// update local map only if transform is large enough
+						std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+						for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+						{
+							if(newSignature->getWords3().count(*iter) == 1 &&
+							   uniqueCorrespondences.find(*iter) == uniqueCorrespondences.end())
+							{
+								const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+								if(pcl::isFinite(pt) &&
+								   (pt.x != 0 || pt.y != 0 || pt.z != 0) &&
+								   (this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
+								{
+									pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
+									localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
+								}
+							}
+						}
+						while(localMap_.size() && (int)localMap_.size() > this->getLocalHistory() && _memory->getStMem().size()>1)
+						{
+							std::list<int> deletedWords;
+							_memory->deleteLocation(*_memory->getStMem().begin(), &deletedWords);
+							for(std::list<int>::iterator iter = deletedWords.begin(); iter!=deletedWords.end(); ++iter)
+							{
+								localMap_.erase(*iter);
+							}
+						}
+					}
+					else
+					{
+						_memory->deleteLocation(newSignature->id());
+					}
+				}
 			}
 		}
 		else if(!previousSignature && newSignature)
 		{
+			localMap_.clear();
 			output.setIdentity();
+
+			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+			for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+			{
+				if(newSignature->getWords3().count(*iter) == 1)
+				{
+					const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+					if(pcl::isFinite(pt) &&
+					   (pt.x != 0 || pt.y != 0 || pt.z != 0) &&
+					   (this->getMaxDepth() <= 0 || (pt.x <= this->getMaxDepth())))
+					{
+						localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt));
+					}
+				}
+			}
 		}
 
 		_memory->emptyTrash();
 	}
 
-	UINFO("Odom update time = %fs features=%d inliers=%d/%d",
+	UINFO("Odom update time = %fs features=%d inliers=%d/%d dict=%d nodes=%d",
 			timer.elapsed(),
 			descriptors.rows,
 			inliers,
-			correspondences);
+			correspondences,
+			(int)_memory->getVWDictionary()->getVisualWords().size(),
+			(int)_memory->getStMem().size());
 	return output;
 }
 
 // OdometryICP
-OdometryICP::OdometryICP(
+OdometryICP::OdometryICP(int decimation,
-		int decimation,
 		float voxelSize,
-		float samples,
+		int samples,
 		float maxCorrespondenceDistance,
-		int	maxIterations,
+		int maxIterations,
 		float maxFitness,
-		float maxDepth,
+		const ParametersMap & odometryParameter) :
-		float linearUpdate,
+	Odometry(odometryParameter),
-		float angularUpdate,
-		int resetCoutdown) :
-	Odometry(0, 0, 0, 0, 0, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
 	_decimation(decimation),
 	_voxelSize(voxelSize),
 	_samples(samples),
@@ -556,25 +368,6 @@ OdometryICP::OdometryICP(
 	_maxFitness(maxFitness),
 	_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
 {
-
-}
-
-OdometryICP::OdometryICP(const ParametersMap & parameters) :
-	Odometry(parameters),
-	_decimation(Parameters::defaultOdomICPDecimation()),
-	_voxelSize(Parameters::defaultOdomICPVoxelSize()),
-	_samples(Parameters::defaultOdomICPSamples()),
-	_maxCorrespondenceDistance(Parameters::defaultOdomICPCorrespondencesDistance()),
-	_maxIterations(Parameters::defaultOdomICPIterations()),
-	_maxFitness(Parameters::defaultOdomICPMaxFitness()),
-	_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
-{
-	Parameters::parse(parameters, Parameters::kOdomICPDecimation(), _decimation);
-	Parameters::parse(parameters, Parameters::kOdomICPVoxelSize(), _voxelSize);
-	Parameters::parse(parameters, Parameters::kOdomICPSamples(), _samples);
-	Parameters::parse(parameters, Parameters::kOdomICPCorrespondencesDistance(), _maxCorrespondenceDistance);
-	Parameters::parse(parameters, Parameters::kOdomICPIterations(), _maxIterations);
-	Parameters::parse(parameters, Parameters::kOdomICPMaxFitness(), _maxFitness);
 }
 
 void OdometryICP::reset()

corelib/src/Rtabmap.cpp

@@ -26,6 +26,7 @@
 #include "rtabmap/core/EpipolarGeometry.h"
 
 #include "rtabmap/core/Memory.h"
+#include "rtabmap/core/VWDictionary.h"
 #include "BayesFilter.h"
 
 #include <rtabmap/utilite/ULogger.h>
@@ -1406,7 +1407,7 @@ bool Rtabmap::process(const Image & image)
 	{
 		lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
 	}
-	dictionarySize = _memory->getVWDictionarySize();
+	dictionarySize = _memory->getVWDictionary()->getVisualWords().size();
 	refWordsCount = (int)signature->getWords().size();
 	refUniqueWordsCount = (int)uUniqueKeys(signature->getWords()).size();
 
@@ -1711,6 +1712,11 @@ bool Rtabmap::process(const Image & image)
 	return true;
 }
 
+bool Rtabmap::process(const cv::Mat & image, int id)
+{
+	return this->process(Image(image, id));
+}
+
 // SETTERS
 void Rtabmap::setTimeThreshold(float maxTimeAllowed)
 {
@@ -1804,11 +1810,6 @@ void Rtabmap::rejectLoopClosure(int oldId, int newId)
 	}
 }
 
-bool Rtabmap::process(const cv::Mat & image, int id)
-{
-	return this->process(Image(image, id));
-}
-
 void Rtabmap::dumpData() const
 {
 	UDEBUG("");

corelib/src/VisualWord.cpp

@@ -24,22 +24,12 @@
 namespace rtabmap
 {
 
-VisualWord::VisualWord(int id, const float * descriptor, int dim, int signatureId) :
+VisualWord::VisualWord(int id, const cv::Mat & descriptor, int signatureId) :
 	_id(id),
+	_descriptor(descriptor),
 	_saved(false),
 	_totalReferences(0)
 {
-	_descriptor = new float[dim];
-	if(_descriptor && descriptor)
-	{
-		memcpy(_descriptor, descriptor, dim*sizeof(float));
-	}
-	else
-	{
-		ULOGGER_ERROR("not enough memory to create the descriptor...");
-	}
-	_dim = dim;
-
 	if(signatureId)
 	{
 		addRef(signatureId);
@@ -48,10 +38,6 @@ VisualWord::VisualWord(int id, const float * descriptor, int dim, int signatureI
 
 VisualWord::~VisualWord()
 {
-	if(_descriptor)
-	{
-		delete [] _descriptor;
-	}
 }
 
 void VisualWord::addRef(int signatureId)

corelib/src/VisualWord.h

@@ -31,7 +31,7 @@ class SignatureSurf;
 class RTABMAP_EXP VisualWord
 {
 public:
-	VisualWord(int id, const float * descriptor, int dim, int signatureId = 0);
+	VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0);
 	~VisualWord();
 
 	void addRef(int signatureId);
@@ -39,8 +39,7 @@ public:
 
 	int getTotalReferences() const {return _totalReferences;}
 	int id() const {return _id;}
-	const float * getDescriptor() const {return _descriptor;}
+	const cv::Mat & getDescriptor() const {return _descriptor;}
-	int getDim() const {return _dim;}
 	const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
 
 	bool isSaved() const {return _saved;}
@@ -48,8 +47,7 @@ public:
 
 private:
 	int _id;
-	float * _descriptor;
+	cv::Mat _descriptor;
-	int _dim;
 	bool _saved; // If it's saved to db
 
 	int _totalReferences;

corelib/src/util3d.cpp

@@ -377,7 +377,8 @@ void 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)
 {
 	std::list<int> ids = uUniqueKeys(words1);
 	// Find pairs
@@ -398,6 +399,10 @@ void findCorrespondences(
 			   (maxDepth <= 0 || (inliers1[oi].x <= maxDepth && inliers2[oi].x<=maxDepth)))
 			{
 				++oi;
+				if(uniqueCorrespondences)
+				{
+					uniqueCorrespondences->insert(*iter);
+				}
 			}
 		}
 	}
@@ -604,6 +609,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
 	return output;
 }
 
+pcl::PointXYZ RTABMAP_EXP transformPoint(
+		const pcl::PointXYZ & pt,
+		const Transform & transform)
+{
+	return pcl::transformPoint(pt, transformToEigen3f(transform));
+}
+
+pcl::PointXYZRGB RTABMAP_EXP transformPoint(
+		const pcl::PointXYZRGB & pt,
+		const Transform & transform)
+{
+	return pcl::transformPoint(pt, transformToEigen3f(transform));
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float depthConstant,

examples/RGBDMapping/main.cpp

@@ -53,7 +53,7 @@ int main(int argc, char * argv[])
 	}
 
 	// Create an odometry thread to process camera events, it will send OdometryEvent.
-	OdometryThread odomThread(new OdometryBOW(0)); // 0=SURF 1=SIFT
+	OdometryThread odomThread(new OdometryBOW()); // 0=SURF 1=SIFT
 
 
 	// Create RTAB-Map to process OdometryEvent

guilib/include/rtabmap/gui/OdometryViewer.h

@@ -37,6 +37,7 @@ private:
 	UMutex dataMutex_;
 	std::list<rtabmap::Image> data_;
 	int dataQuality_;
+	Transform lastOdomPose_;
 	UTimer timer_;
 	int maxClouds_;
 	float voxelSize_;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -82,12 +82,6 @@ public:
 		kSrcOpenNI2
 	};
 
-	enum OdomType {
-		kOdomBIN,
-		kOdomBOW,
-		kOdomICP
-	};
-
 public:
 	PreferencesDialog(QWidget * parent = 0);
 	virtual ~PreferencesDialog();
@@ -148,7 +142,6 @@ public:
 	bool isSourceImageUsed() const;
 	bool isSourceDatabaseUsed() const;
 	bool isSourceOpenniUsed() const;
-	OdomType getOdometryType() const;
 	int getSourceImageType() const;
 	QString getSourceImageTypeStr() const;
 	int getSourceWidth() const;
@@ -259,7 +252,7 @@ private:
 	void addParameters(const QGroupBox * box);
 	QList<QGroupBox*> getGroupBoxes();
 	void readSettingsBegin();
-	void testOdometry(OdomType type);
+	void testOdometry(int type);
 
 protected:
 	rtabmap::ParametersMap _parameters;

guilib/src/MainWindow.cpp

@@ -1963,22 +1963,7 @@ void MainWindow::startDetection()
 				UERROR("OdomThread must be already deleted here?!");
 				delete _odomThread;
 			}
-			Odometry * odom;
+			Odometry * odom = new OdometryBOW(parameters);
-			if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBIN)
-			{
-				UINFO("Using odometry FAST/BRIEF");
-				odom = new OdometryBinary(parameters);
-			}
-			else if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBOW)
-			{
-				UINFO("Using odometry SIFT/SURF");
-				odom = new OdometryBOW(parameters);
-			}
-			else
-			{
-				UINFO("Using odometry ICP");
-				odom = new OdometryICP(parameters);
-			}
 			_odomThread = new OdometryThread(odom);
 
 			UEventsManager::addHandler(_odomThread);
@@ -2057,22 +2042,7 @@ void MainWindow::startDetection()
 				UERROR("OdomThread must be already deleted here?!");
 				delete _odomThread;
 			}
-			Odometry * odom;
+			Odometry * odom = new OdometryBOW(parameters);
-			if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBIN)
-			{
-				UINFO("Using odometry FAST/BRIEF");
-				odom = new OdometryBinary(parameters);
-			}
-			else if(_preferencesDialog->getOdometryType() == PreferencesDialog::kOdomBOW)
-			{
-				UINFO("Using odometry SIFT/SURF");
-				odom = new OdometryBOW(parameters);
-			}
-			else
-			{
-				UINFO("Using odometry ICP");
-				odom = new OdometryICP(parameters);
-			}
 			_odomThread = new OdometryThread(odom);
 
 			UEventsManager::addHandler(_odomThread);

guilib/src/OdometryViewer.cpp

@@ -23,6 +23,7 @@ namespace rtabmap {
 
 OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, int qualityWarningThr, QWidget * parent) :
 		CloudViewer(parent),
+		lastOdomPose_(Transform::getIdentity()),
 		maxClouds_(maxClouds),
 		voxelSize_(voxelSize),
 		decimation_(decimation),
@@ -60,9 +61,9 @@ void OdometryViewer::processData()
 	}
 	dataMutex_.unlock();
 
-	if(!data.empty() && this->isVisible())
+	if(!data.image().empty() && !data.depth().empty() && data.depthConstant()>0.0f && this->isVisible())
 	{
-		UINFO("New pose = %s", data.pose().prettyPrint().c_str());
+		UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
 
 		// visualization: buffering the clouds
 		// Create the new cloud
@@ -83,30 +84,44 @@ void OdometryViewer::processData()
 
 		cloud = util3d::transformPointCloud(cloud, data.localTransform());
 
-		data.id()?id_=data.id():++id_;
+		if(!data.pose().isNull())
-
-		clouds_.insert(std::make_pair(id_, cloud));
-
-		while(maxClouds_>0 && (int)clouds_.size() > maxClouds_)
 		{
-			this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
+			lastOdomPose_ = data.pose();
-			clouds_.erase(clouds_.begin());
+			if(this->getAddedClouds().contains("cloudtmp"))
-		}
+			{
+				this->removeCloud("cloudtmp");
+			}
 
-		if(clouds_.size())
+			data.id()?id_=data.id():++id_;
-		{
-			this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
-		}
 
-		this->updateCameraPosition(data.pose());
+			clouds_.insert(std::make_pair(id_, cloud));
 
-		if(qualityWarningThr_ && quality && quality < qualityWarningThr_)
+			while(maxClouds_>0 && (int)clouds_.size() > maxClouds_)
-		{
+			{
-			this->setBackgroundColor(Qt::darkYellow);
+				this->removeCloud(uFormat("cloud%d", clouds_.begin()->first));
+				clouds_.erase(clouds_.begin());
+			}
+
+			if(clouds_.size())
+			{
+				this->addCloud(uFormat("cloud%d", clouds_.rbegin()->first), clouds_.rbegin()->second, data.pose());
+			}
+
+			this->updateCameraPosition(data.pose());
+
+			if(qualityWarningThr_ && quality && quality < qualityWarningThr_)
+			{
+				this->setBackgroundColor(Qt::darkYellow);
+			}
+			else
+			{
+				this->setBackgroundColor(Qt::black);
+			}
 		}
 		else
 		{
-			this->setBackgroundColor(Qt::black);
+			this->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
+			this->setBackgroundColor(Qt::darkRed);
 		}
 
 		this->render();
@@ -121,31 +136,22 @@ void OdometryViewer::handleEvent(UEvent * event)
 		{
 			rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
 
-			if(odomEvent->isValid())
+			bool empty = false;
+			dataMutex_.lock();
+			if(data_.empty())
 			{
-				bool empty = false;
+				data_.push_back(odomEvent->data());
-				dataMutex_.lock();
+				empty= true;
-				if(data_.empty())
-				{
-					data_.push_back(odomEvent->data());
-					empty= true;
-				}
-				else
-				{
-					data_.back() = odomEvent->data();
-				}
-				dataQuality_ = odomEvent->quality();
-				dataMutex_.unlock();
-				if(empty)
-				{
-					QMetaObject::invokeMethod(this, "processData");
-				}
 			}
 			else
 			{
-				//UWARN("odom=%fs, Cannot compute odometry!!!", timer_.restart());
+				data_.back() = odomEvent->data();
-				QMetaObject::invokeMethod(this, "setBackgroundColor", Q_ARG(QColor, Qt::darkRed));
+			}
-				QMetaObject::invokeMethod(this, "render");
+			dataQuality_ = odomEvent->quality();
+			dataMutex_.unlock();
+			if(empty)
+			{
+				QMetaObject::invokeMethod(this, "processData");
 			}
 		}
 	}

guilib/src/PreferencesDialog.cpp

@@ -42,6 +42,7 @@
 #include "rtabmap/core/CameraThread.h"
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/Memory.h"
+#include "rtabmap/core/VWDictionary.h"
 
 #include "rtabmap/gui/LoopClosureViewer.h"
 
@@ -73,8 +74,25 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	if(cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
+		_ui->surf_checkBox_gpuVersion->setChecked(false);
 		_ui->surf_checkBox_gpuVersion->setEnabled(false);
 		_ui->label_surf_checkBox_gpuVersion->setEnabled(false);
+		_ui->surf_doubleSpinBox_gpuKeypointsRatio->setEnabled(false);
+		_ui->label_surf_checkBox_gpuKeypointsRatio->setEnabled(false);
+
+		_ui->fastGpu->setChecked(false);
+		_ui->fastGpu->setEnabled(false);
+		_ui->label_fastGPU->setEnabled(false);
+		_ui->fastKeypointRatio->setEnabled(false);
+		_ui->label_fastGPUKptRatio->setEnabled(false);
+
+		_ui->checkBox_ORBGpu->setChecked(false);
+		_ui->checkBox_ORBGpu->setEnabled(false);
+		_ui->label_orbGpu->setEnabled(false);
+
+		// remove BruteForceGPU option
+		_ui->comboBox_dictionary_strategy->removeItem(4);
+		_ui->odom_bin_nn->removeItem(4);
 	}
 
 	_ui->predictionPlot->showLegend(false);
@@ -317,11 +335,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_dictionary_strategy->setObjectName(Parameters::kKpNNStrategy().c_str());
 	_ui->checkBox_dictionary_incremental->setObjectName(Parameters::kKpIncrementalDictionary().c_str());
 	_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
-	_ui->checkBox_dictionary_minDistUsed->setObjectName(Parameters::kKpMinDistUsed().c_str());
-	_ui->surf_doubleSpinBox_matchThr->setObjectName(Parameters::kKpMinDist().c_str());
-	_ui->checkBox_dictionary_nndrUsed->setObjectName(Parameters::kKpNndrUsed().c_str());
 	_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
-	_ui->surf_spinBox_maxLeafs->setObjectName(Parameters::kKpMaxLeafs().c_str());
 	_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
 	_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpWordsPerImage().c_str());
 	_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
@@ -338,6 +352,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_surfExtended->setObjectName(Parameters::kSURFExtended().c_str());
 	_ui->surf_checkBox_upright->setObjectName(Parameters::kSURFUpright().c_str());
 	_ui->surf_checkBox_gpuVersion->setObjectName(Parameters::kSURFGpuVersion().c_str());
+	_ui->surf_doubleSpinBox_gpuKeypointsRatio->setObjectName(Parameters::kSURFGpuKeypointsRatio().c_str());
 
 	//SIFT detector
 	_ui->sift_spinBox_nFeatures->setObjectName(Parameters::kSIFTNFeatures().c_str());
@@ -346,6 +361,32 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->sift_doubleSpinBox_edgeThr->setObjectName(Parameters::kSIFTEdgeThreshold().c_str());
 	_ui->sift_doubleSpinBox_sigma->setObjectName(Parameters::kSIFTSigma().c_str());
 
+	//BRIEF descriptor
+	_ui->briefBytes->setObjectName(Parameters::kBRIEFBytes().c_str());
+
+	//FAST detector
+	_ui->fastSuppressNonMax->setObjectName(Parameters::kFASTNonmaxSuppression().c_str());
+	_ui->fastThreshold->setObjectName(Parameters::kFASTThreshold().c_str());
+	_ui->fastGpu->setObjectName(Parameters::kFASTGpu().c_str());
+	_ui->fastKeypointRatio->setObjectName(Parameters::kFASTGpuKeypointsRatio().c_str());
+
+	//ORB detector
+	_ui->spinBox_ORBNFeatures->setObjectName(Parameters::kORBNFeatures().c_str());
+	_ui->doubleSpinBox_ORBScaleFactor->setObjectName(Parameters::kORBScaleFactor().c_str());
+	_ui->spinBox_ORBNLevels->setObjectName(Parameters::kORBNLevels().c_str());
+	_ui->spinBox_ORBEdgeThreshold->setObjectName(Parameters::kORBEdgeThreshold().c_str());
+	_ui->spinBox_ORBFirstLevel->setObjectName(Parameters::kORBFirstLevel().c_str());
+	_ui->spinBox_ORBWTA_K->setObjectName(Parameters::kORBWTA_K().c_str());
+	_ui->spinBox_ORBScoreType->setObjectName(Parameters::kORBScoreType().c_str());
+	_ui->spinBox_ORBPatchSize->setObjectName(Parameters::kORBPatchSize().c_str());
+	_ui->checkBox_ORBGpu->setObjectName(Parameters::kORBGpu().c_str());
+
+	//FREAK descriptor
+	_ui->checkBox_FREAKOrientationNormalized->setObjectName(Parameters::kFREAKOrientationNormalized().c_str());
+	_ui->checkBox_FREAKScaleNormalized->setObjectName(Parameters::kFREAKScaleNormalized().c_str());
+	_ui->doubleSpinBox_FREAKPatternScale->setObjectName(Parameters::kFREAKPatternScale().c_str());
+	_ui->spinBox_FREAKNOctaves->setObjectName(Parameters::kFREAKNOctaves().c_str());
+
 	// verifyHypotheses
 	_ui->comboBox_vh_strategy->setObjectName(Parameters::kRtabmapVhStrategy().c_str());
 	_ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str());
@@ -393,26 +434,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_linearUpdate->setObjectName(Parameters::kOdomLinearUpdate().c_str());
 	_ui->odom_angularUpdate->setObjectName(Parameters::kOdomAngularUpdate().c_str());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
+	_ui->odom_localHistory->setObjectName(Parameters::kOdomLocalHistory().c_str());
 	_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxWords().c_str());
 	_ui->odom_inlierDistance->setObjectName(Parameters::kOdomInlierDistance().c_str());
 	_ui->odom_iterations->setObjectName(Parameters::kOdomIterations().c_str());
 	_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
 	_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
 	_ui->odom_ratio->setObjectName(Parameters::kOdomWordsRatio().c_str());
-	_ui->stackedWidget_odom->setCurrentIndex(_ui->odom_type->currentIndex());
+	_ui->odom_bin_nn->setObjectName(Parameters::kOdomNearestNeighbor().c_str());
-	connect(_ui->odom_type, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odom, SLOT(setCurrentIndex(int)));
+	_ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomNNDR().c_str());
-
-	_ui->odom_bin_briefBytes->setObjectName(Parameters::kOdomBinBriefBytes().c_str());
-	_ui->odom_bin_fastSuppressNonMax->setObjectName(Parameters::kOdomBinFastNonmaxSuppression().c_str());
-	_ui->odom_bin_fastThreshold->setObjectName(Parameters::kOdomBinFastThreshold().c_str());
-	_ui->odom_bin_bruteForceMatching->setObjectName(Parameters::kOdomBinBruteForceMatching().c_str());
-
-	_ui->odom_icpDecimation->setObjectName(Parameters::kOdomICPDecimation().c_str());
-	_ui->odom_icpVoxelSize->setObjectName(Parameters::kOdomICPVoxelSize().c_str());
-	_ui->odom_icpSamples->setObjectName(Parameters::kOdomICPSamples().c_str());
-	_ui->odom_icpMaxCorrespondenceDistance->setObjectName(Parameters::kOdomICPCorrespondencesDistance().c_str());
-	_ui->odom_icpIterations->setObjectName(Parameters::kOdomICPIterations().c_str());
-	_ui->odom_icpMaxFitness->setObjectName(Parameters::kOdomICPMaxFitness().c_str());
 
 	setupSignals();
 	// custom signals
@@ -791,7 +821,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		// match the advanced (spin and doubleSpin boxes)
 		_ui->general_doubleSpinBox_timeThr->setValue(Parameters::defaultRtabmapTimeThr());
 		_ui->general_doubleSpinBox_hardThr->setValue(Parameters::defaultRtabmapLoopThr());
-		_ui->surf_doubleSpinBox_hessianThr->setValue(Parameters::defaultSURFHessianThreshold());
 		_ui->doubleSpinBox_similarityThreshold->setValue(Parameters::defaultMemRehearsalSimilarity());
 		_ui->general_spinBox_imagesBufferSize->setValue(Parameters::defaultRtabmapImageBufferSize());
 		_ui->general_spinBox_maxStMemSize->setValue(Parameters::defaultMemSTMSize());
@@ -813,6 +842,21 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			{
 				this->resetSettings((QGroupBox*)children.at(i));
 			}
+			else if(qobject_cast<const QStackedWidget*>(children.at(i)))
+			{
+				QStackedWidget * stackedWidget = (QStackedWidget*)children.at(i);
+				for(int j=0; j<stackedWidget->count(); ++j)
+				{
+					const QObjectList & children2 = stackedWidget->widget(j)->children();
+					for(int k=0; k<children2.size(); ++k)
+					{
+						if(qobject_cast<QGroupBox *>(children2.at(k)))
+						{
+							this->resetSettings((QGroupBox*)children2.at(k));
+						}
+					}
+				}
+			}
 		}
 
 		if(groupBox->findChild<QLineEdit*>(_ui->lineEdit_kp_roi->objectName()))
@@ -1294,7 +1338,30 @@ void PreferencesDialog::writeCoreSettings(const QString & filePath)
 
 bool PreferencesDialog::validateForm()
 {
-	//TODO...
+	//verify odom type vs nearest neighbor approach
+	if(_ui->comboBox_dictionary_strategy->currentIndex() == VWDictionary::kNNFlannLSH)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("With the selected feature type (SURF or SIFT), parameter \"Visual Word->Nearest Neighbor\" "
+				   "cannot be LSH (used for binary descriptor). KD-tree is set instead."));
+		_ui->comboBox_dictionary_strategy->setCurrentIndex(VWDictionary::kNNFlannKdTree);
+	}
+
+	if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannLSH && _ui->odom_type->currentIndex() <= 1)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("With the selected feature type (SURF or SIFT), parameter \"Odometry->Nearest Neighbor\" "
+				   "cannot be LSH (used for binary descriptor). KD-tree is set instead."));
+		_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannKdTree);
+	}
+	else if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannKdTree && _ui->odom_type->currentIndex() >1)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Odometry->Nearest Neighbor\" "
+				   "cannot be KD-Tree (used for float descriptor). LSH is set instead."));
+		_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannLSH);
+	}
+
 	return true;
 }
 
@@ -1642,60 +1709,67 @@ void PreferencesDialog::openDatabaseViewer()
 void PreferencesDialog::setParameter(const std::string & key, const std::string & value)
 {
 	UDEBUG("%s=%s", key.c_str(), value.c_str());
-	QObject * obj = _ui->stackedWidget->findChild<QObject*>(key.c_str());
+	QWidget * obj = _ui->stackedWidget->findChild<QWidget*>(key.c_str());
 	if(obj)
 	{
-		QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
+		if(obj->isEnabled())
-		QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
-		QComboBox * combo = qobject_cast<QComboBox *>(obj);
-		QCheckBox * check = qobject_cast<QCheckBox *>(obj);
-		QRadioButton * radio = qobject_cast<QRadioButton *>(obj);
-		QLineEdit * lineEdit = qobject_cast<QLineEdit *>(obj);
-		QGroupBox * groupBox = qobject_cast<QGroupBox *>(obj);
-		bool ok;
-		if(spin)
 		{
-			spin->setValue(QString(value.c_str()).toInt(&ok));
+			QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
-			if(!ok)
+			QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
+			QComboBox * combo = qobject_cast<QComboBox *>(obj);
+			QCheckBox * check = qobject_cast<QCheckBox *>(obj);
+			QRadioButton * radio = qobject_cast<QRadioButton *>(obj);
+			QLineEdit * lineEdit = qobject_cast<QLineEdit *>(obj);
+			QGroupBox * groupBox = qobject_cast<QGroupBox *>(obj);
+			bool ok;
+			if(spin)
 			{
-				UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				spin->setValue(QString(value.c_str()).toInt(&ok));
+				if(!ok)
+				{
+					UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				}
 			}
-		}
+			else if(doubleSpin)
-		else if(doubleSpin)
-		{
-			doubleSpin->setValue(QString(value.c_str()).toDouble(&ok));
-			if(!ok)
 			{
-				UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				doubleSpin->setValue(QString(value.c_str()).toDouble(&ok));
+				if(!ok)
+				{
+					UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				}
 			}
-		}
+			else if(combo)
-		else if(combo)
-		{
-			combo->setCurrentIndex(QString(value.c_str()).toInt(&ok));
-			if(!ok)
 			{
-				UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				combo->setCurrentIndex(QString(value.c_str()).toInt(&ok));
+				if(!ok)
+				{
+					UERROR("Conversion failed from \"%s\" for parameter %s", value.c_str(), key.c_str());
+				}
+			}
+			else if(check)
+			{
+				check->setChecked(uStr2Bool(value.c_str()));
+			}
+			else if(radio)
+			{
+				radio->setChecked(uStr2Bool(value.c_str()));
+			}
+			else if(lineEdit)
+			{
+				lineEdit->setText(value.c_str());
+			}
+			else if(groupBox)
+			{
+				groupBox->setChecked(uStr2Bool(value.c_str()));
+			}
+			else
+			{
+				ULOGGER_WARN("QObject called %s can't be cast to a supported widget", key.c_str());
 			}
-		}
-		else if(check)
-		{
-			check->setChecked(uStr2Bool(value.c_str()));
-		}
-		else if(radio)
-		{
-			radio->setChecked(uStr2Bool(value.c_str()));
-		}
-		else if(lineEdit)
-		{
-			lineEdit->setText(value.c_str());
-		}
-		else if(groupBox)
-		{
-			groupBox->setChecked(uStr2Bool(value.c_str()));
 		}
 		else
 		{
-			ULOGGER_WARN("QObject called %s can't be cast to a supported widget", key.c_str());
+			UDEBUG("Ignoring parameter %s because it is disabled.", key.c_str());
 		}
 	}
 	else
@@ -1781,35 +1855,29 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 						this->addParameters(_ui->groupBox_vh_epipolar2);
 					}
 				}
-				else if(comboBox == _ui->comboBox_detector_strategy)
+				else if(comboBox == _ui->comboBox_detector_strategy || comboBox == _ui->odom_type)
 				{
-					if(value == 0) // 0 surf
+					if(value == 0) //  surf
 					{
-						this->addParameters(_ui->groupBox_detector_surf);
+						this->addParameters(_ui->groupBox_detector_surf2);
-						_ui->stackedWidget_visualWord->setCurrentIndex(0);
 					}
-					else if(value == 1) // 1 sift
+					else if(value == 1) //  sift
 					{
-						this->addParameters(_ui->groupBox_detector_sift);
+						this->addParameters(_ui->groupBox_detector_sift2);
-						_ui->stackedWidget_visualWord->setCurrentIndex(1);
 					}
-				}
+					else if(value == 2) //  orb
-				else if(comboBox == _ui->odom_type)
-				{
-					if(value == 0) // 0 bow
 					{
-						this->addParameters(_ui->groupBox_odom_bow);
+						this->addParameters(_ui->groupBox_detector_orb2);
-						_ui->stackedWidget_odom->setCurrentIndex(0);
 					}
-					else if(value == 1) // 1 fast
+					else if(value == 3) //  fast+freak
 					{
-						this->addParameters(_ui->groupBox_odom_fast);
+						this->addParameters(_ui->groupBox_detector_fast2);
-						_ui->stackedWidget_odom->setCurrentIndex(1);
+						this->addParameters(_ui->groupBox_detector_freak2);
 					}
-					else if(value == 2) // 1 fast
+					else if(value == 4) //  fast+brief
 					{
-						this->addParameters(_ui->groupBox_odom_icp);
+						this->addParameters(_ui->groupBox_detector_fast2);
-						_ui->stackedWidget_odom->setCurrentIndex(2);
+						this->addParameters(_ui->groupBox_detector_brief2);
 					}
 				}
 				else if(comboBox == _ui->globalDetection_icpType)
@@ -2446,18 +2514,6 @@ bool PreferencesDialog::isSourceOpenniUsed() const
 	return _ui->groupBox_sourceOpenni->isChecked();
 }
 
-PreferencesDialog::OdomType PreferencesDialog::getOdometryType() const
-{
-	if(_ui->odom_type->currentIndex() == 0)
-	{
-		return kOdomBOW;
-	}
-	else if(_ui->odom_type->currentIndex() == 1)
-	{
-		return kOdomBIN;
-	}
-	return kOdomICP;
-}
 
 int PreferencesDialog::getSourceImageType() const
 {
@@ -2686,21 +2742,10 @@ void PreferencesDialog::setSLAMMode(bool enabled)
 
 void PreferencesDialog::testOdometry()
 {
-	if(_ui->odom_type->currentIndex() == 0)
+	testOdometry(_ui->odom_type->currentIndex());
-	{
-		testOdometry(kOdomBOW);
-	}
-	else if(_ui->odom_type->currentIndex() == 1)
-	{
-		testOdometry(kOdomBIN);
-	}
-	else // ICP
-	{
-		testOdometry(kOdomICP);
-	}
 }
 
-void PreferencesDialog::testOdometry(OdomType type)
+void PreferencesDialog::testOdometry(int type)
 {
 	UASSERT(_odomThread == 0 && _cameraThread == 0);
 
@@ -2750,27 +2795,15 @@ void PreferencesDialog::testOdometry(OdomType type)
 
 	if(camera)
 	{
-		Odometry * odometry;
 		ParametersMap parameters = this->getAllParameters();
-		if(type == kOdomBIN)
+		Odometry * odometry = new OdometryBOW(parameters);
-		{
-			odometry = new OdometryBinary(parameters);
-		}
-		else if(type == kOdomICP)
-		{
-			odometry = new OdometryICP(parameters);
-		}
-		else // kOdomBOW
-		{
-			odometry = new OdometryBOW(parameters);
-		}
 
 		_odomThread = new OdometryThread(odometry); // take ownership of odometry
 
 		QWidget * window = new QWidget(this, Qt::Popup);
 		window->setAttribute(Qt::WA_DeleteOnClose);
 		window->setWindowFlags(Qt::Dialog);
-		window->setWindowTitle(tr("%1 Odometry viewer").arg(type==kOdomBIN?"Binary":"Bag-of-words"));
+		window->setWindowTitle(tr("Odometry viewer"));
 		window->setMinimumWidth(800);
 		window->setMinimumHeight(600);
 		connect( window, SIGNAL(destroyed(QObject*)), this, SLOT(cleanOdometryTest()) );

tools/EpipolarGeometry/main.cpp

@@ -192,12 +192,11 @@ int main(int argc, char** argv)
    ParametersMap param;
    param.insert(ParametersPair(Parameters::kSURFExtended(), "true"));
    param.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "100"));
-   SURFDetector keypointDetector(param);
+   SURF detector(param);
-   SURFDescriptor descriptorExtractor(param);
+   std::vector<cv::KeyPoint> kpts1 = detector.generateKeypoints(image1);
-   std::vector<cv::KeyPoint> kpts1 = keypointDetector.generateKeypoints(image1);
+   std::vector<cv::KeyPoint> kpts2 = detector.generateKeypoints(image2);
-   std::vector<cv::KeyPoint> kpts2 = keypointDetector.generateKeypoints(image2);
+   cv::Mat descriptors1 = detector.generateDescriptors(image1, kpts1);
-   cv::Mat descriptors1 = descriptorExtractor.generateDescriptors(image1, kpts1);
+   cv::Mat descriptors2 = detector.generateDescriptors(image2, kpts2);
-   cv::Mat descriptors2 = descriptorExtractor.generateDescriptors(image2, kpts2);
    UINFO("detect/extract features = %d ms", timer.elapsed());
 
    timer.start();

tools/OdometryViewer/main.cpp

@@ -2,11 +2,13 @@
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/core/Odometry.h>
 #include <rtabmap/gui/OdometryViewer.h>
 #include <rtabmap/core/CameraThread.h>
 #include <rtabmap/core/CameraRGBD.h>
 #include <rtabmap/core/DBReader.h>
+#include <rtabmap/core/VWDictionary.h>
 #include <QtGui/QApplication>
 
 void showUsage()
@@ -14,8 +16,9 @@ void showUsage()
 	printf("\nUsage:\n"
 			"odometryViewer [options]\n"
 			"Options:\n"
-			"  -bow #                    Use bag-of-words odometry (default 0): 0=SURF, 1=SIFT\n"
+			"  -o #                      Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF\n"
-			"  -bin                      Use binary odometry (FAST+BRIEF)\n"
+			"  -nn #                     Nearest neighbor strategy (default 1): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
+			"  -nndr #                   Nearest neighbor distance ratio (default 0.7)\n"
 			"  -icp                      Use ICP odometry\n"
 			"\n"
 			"  -hz #.#                   Camera rate (default 0, 0 means as fast as the camera can)\n"
@@ -32,10 +35,11 @@ void showUsage()
 			"  -lu #                     Linear update (default 0.0 m)\n"
 			"  -au #                     Angular update (default 0.0 radian)\n"
 			"  -reset #                  Reset countdown (default 0 = disabled)\n"
+			"  -gpu                      Use GPU\n"
+			"  -lh #                     Local history (default 1)\n"
 			"\n"
-			"  -bin_brief_bytes #        BRIEF bytes (default 32)\n"
+			"  -brief_bytes #        BRIEF bytes (default 32)\n"
-			"  -bin_fast_thr #           FAST threshold (default 30)\n"
+			"  -fast_thr #           FAST threshold (default 30)\n"
-			"  -bin_lsh                  Use nearest neighbor LSH (default brute force hamming)\n"
 			"\n"
 			"  -d #                      ICP decimation (default 4)\n"
 			"  -v #                      ICP voxel size (default 0.005)\n"
@@ -45,9 +49,9 @@ void showUsage()
 			"  -debug                    Log debug messages\n"
 			"\n"
 			"Examples:\n"
-			"  odometryViewer -bow 0                                SURF example\n"
+			"  odometryViewer -odom 0 -lh 5000                      SURF example\n"
-			"  odometryViewer -bow 1                                SIFT example\n"
+			"  odometryViewer -odom 1 -lh 10000                     SIFT example\n"
-			"  odometryViewer -bin -hz 10                           FAST/BRIEF example\n"
+			"  odometryViewer -odom 4 -nn 2 -lh 1000                FAST/BRIEF example\n"
 			"  odometryViewer -icp -in 0.05 -i 30                   ICP example\n");
 	exit(1);
 }
@@ -60,8 +64,10 @@ int main (int argc, char * argv[])
 	// parse arguments
 	float rate = 0.0;
 	std::string inputDatabase;
-	int odomType = 0; // 0=bow 1=bin 2=ICP
+	int odomType = 0;
-	int bowType = 0;
+	bool icp = false;
+	int nnType =1;
+	float nndr = 0.7f;
 	float distance = 0.005;
 	int maxWords = 0;
 	int minInliers = 20;
@@ -78,19 +84,53 @@ int main (int argc, char * argv[])
 	int maxClouds = 10;
 	int briefBytes = 32;
 	int fastThr = 30;
-	bool useLSH = false;
 	float sec = 0.0f;
+	bool gpu = false;
+	int localHistory = 0;
 
 	for(int i=1; i<argc; ++i)
 	{
-		if(strcmp(argv[i], "-bow") == 0)
+		if(strcmp(argv[i], "-o") == 0)
 		{
 			++i;
 			if(i < argc)
 			{
-				bowType = std::atoi(argv[i]);
+				odomType = std::atoi(argv[i]);
-				odomType = 0;
+				if(odomType < 0 || odomType > 4)
-				if(bowType < 0 || bowType > 1)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-nn") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				nnType = std::atoi(argv[i]);
+				if(nnType < 0 || nnType > 4)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-nndr") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				nndr = std::atof(argv[i]);
+				if(nndr < 0.0f)
 				{
 					showUsage();
 				}
@@ -391,7 +431,29 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-bin_brief_bytes") == 0)
+		if(strcmp(argv[i], "-gpu") == 0)
+		{
+			gpu = true;
+			continue;
+		}
+		if(strcmp(argv[i], "-lh") == 0)
+		{
+			++i;
+			if(i < argc)
+			{
+				localHistory = std::atoi(argv[i]);
+				if(fitness <= 0)
+				{
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+			continue;
+		}
+		if(strcmp(argv[i], "-brief_bytes") == 0)
 		{
 			++i;
 			if(i < argc)
@@ -408,7 +470,7 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-bin_fast_thr") == 0)
+		if(strcmp(argv[i], "-fast_thr") == 0)
 		{
 			++i;
 			if(i < argc)
@@ -425,19 +487,9 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-bin_lsh") == 0)
-		{
-			useLSH = true;
-			continue;
-		}
-		if(strcmp(argv[i], "-bin") == 0)
-		{
-			odomType = 1;
-			continue;
-		}
 		if(strcmp(argv[i], "-icp") == 0)
 		{
-			odomType = 2;
+			icp = true;
 			continue;
 		}
 		if(strcmp(argv[i], "-debug") == 0)
@@ -450,6 +502,17 @@ int main (int argc, char * argv[])
 		showUsage();
 	}
 
+	if(odomType > 1 && nnType == rtabmap::VWDictionary::kNNFlannKdTree)
+	{
+		UERROR("You set \"-o %d\" (binary descriptor), you must use \"-nn 2\" (any \"-nn\" other than kNNFlannKdTree)", odomType);
+		showUsage();
+	}
+	else if(odomType <= 1 && nnType == rtabmap::VWDictionary::kNNFlannLSH)
+	{
+		UERROR("You set \"-o %d\" (float descriptor), you must use \"-nn 1\" (any \"-nn\" other than kNNFlannLSH)", odomType);
+		showUsage();
+	}
+
 	if(inputDatabase.size())
 	{
 		UINFO("Using database input \"%s\"", inputDatabase.c_str());
@@ -458,78 +521,131 @@ int main (int argc, char * argv[])
 	{
 		UINFO("Using OpenNI camera");
 	}
-	UINFO("Camera rate = %f Hz", rate);
+
-	UINFO("Maximum clouds shown = %d", maxClouds);
+	std::string odomName;
-	UINFO("Delay = %f s", sec);
+	if(odomType == 0)
-	UINFO("Odometry used = %s", odomType==0?bowType==0?"Bag-of-words SURF":"Bag-of-words SIFT":odomType==1?"Binary (FAST+BRIEF)":"ICP");
+	{
-	UINFO("Inlier/ICP maximum correspondences distance = %f", distance);
+		odomName = "SURF";
-	UINFO("Max features = %d", maxWords);
+	}
-	UINFO("Min inliers = %d", minInliers);
+	else if(odomType == 1)
-	UINFO("RANSAC/ICP iterations = %d", iterations);
+	{
-	UINFO("Words ratio = %f", wordsRatio);
+		odomName = "SIFT";
-	UINFO("Max depth = %f", maxDepth);
+	}
-	UINFO("Linear update = %f", linearUpdate);
+	else if(odomType == 2)
-	UINFO("Angular update = %f", angularUpdate);
+	{
+		odomName = "ORB";
+	}
+	else if(odomType == 3)
+	{
+		odomName = "FAST+FREAK";
+	}
+	else if(odomType == 4)
+	{
+		odomName = "FAST+BRIEF";
+	}
+
+	if(icp)
+	{
+		odomName= "ICP";
+	}
+
+	std::string nnName;
+	if(nnType == 0)
+	{
+		nnName = "kNNFlannLinear";
+	}
+	else if(nnType == 1)
+	{
+		nnName = "kNNFlannKdTree";
+	}
+	else if(nnType == 2)
+	{
+		nnName= "kNNFlannLSH";
+	}
+	else if(nnType == 3)
+	{
+		nnName= "kNNBruteForce";
+	}
+	else if(nnType == 4)
+	{
+		nnName= "kNNBruteForceGPU";
+	}
+
+	UINFO("Odometry used =           %s", odomName.c_str());
+	UINFO("Camera rate =             %f Hz", rate);
+	UINFO("Maximum clouds shown =    %d", maxClouds);
+	UINFO("Delay =                   %f s", sec);
+	UINFO("Max depth =               %f", maxDepth);
+	UINFO("Linear update =           %f", linearUpdate);
+	UINFO("Angular update =          %f", angularUpdate);
 	UINFO("Reset odometry coutdown = %d", resetCountdown);
-	UINFO("Cloud decimation = %d", decimation);
+	UINFO("Local history =           %d", localHistory);
-	UINFO("Cloud voxel size = %f", voxel);
-	UINFO("Cloud samples = %d", samples);
-	UINFO("Cloud fitness = %f", fitness);
-	UINFO("Binary BRIEF bytes = %d", briefBytes);
-	UINFO("Binary FAST threshold = %f", fastThr);
-	UINFO("Binary LSH = %f", useLSH?"true":"false");
 
 	QApplication app(argc, argv);
 
 	rtabmap::Odometry * odom = 0;
 
-	if(odomType == 0)
+	rtabmap::ParametersMap parameters;
+
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomLinearUpdate(), uNumber2Str(linearUpdate)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomAngularUpdate(), uNumber2Str(angularUpdate)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomLocalHistory(), uNumber2Str(localHistory)));
+
+	if(!icp)
 	{
-		odom = new rtabmap::OdometryBOW(
+		UINFO("Nearest neighbor =         %s", nnName.c_str());
-				bowType,
+		UINFO("Nearest neighbor ratio =  %f", nndr);
-				distance,
+		UINFO("Max features =            %d", maxWords);
-				maxWords,
+		UINFO("Min inliers =             %d", minInliers);
-				minInliers,
+		UINFO("Words ratio =             %f", wordsRatio);
-				iterations,
+		UINFO("Inlier maximum correspondences distance = %f", distance);
-				wordsRatio,
+		UINFO("RANSAC iterations =       %d", iterations);
-				maxDepth,
+		UINFO("GPU =                     %s", gpu?"true":"false");
-				linearUpdate,
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxWords(), uNumber2Str(maxWords)));
-				angularUpdate,
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomWordsRatio(), uNumber2Str(wordsRatio)));
-				resetCountdown);
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
-	}
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
-	else if(odomType == 1)
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
-	{
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomNearestNeighbor(), uNumber2Str(nnType)));
-		odom = new rtabmap::OdometryBinary(
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomNNDR(), uNumber2Str(nndr)));
-				distance,
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomType(), uNumber2Str(odomType)));
-				maxWords,
+		if(odomType == 0)
-				minInliers,
+		{
-				iterations,
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
-				wordsRatio,
+		}
-				maxDepth,
+		if(odomType == 2)
-				linearUpdate,
+		{
-				angularUpdate,
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kORBGpu(), uBool2Str(gpu)));
-				resetCountdown,
+		}
-				briefBytes,
+		if(odomType == 3 || odomType == 4)
-				fastThr,
+		{
-				true,
+			UINFO("FAST threshold =          %d", fastThr);
-				!useLSH);
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
+		}
+		if(odomType == 4)
+		{
+			UINFO("BRIEF bytes =             %d", briefBytes);
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));
+		}
+
+		odom = new rtabmap::OdometryBOW(parameters);
 	}
 	else // ICP
 	{
-		odom = new rtabmap::OdometryICP(
+		UINFO("ICP maximum correspondences distance = %f", distance);
-				decimation,
+		UINFO("ICP iterations =          %d", iterations);
-				voxel,
+		UINFO("Cloud decimation =        %d", decimation);
-				samples,
+		UINFO("Cloud voxel size =        %f", voxel);
-				distance,
+		UINFO("Cloud samples =           %d", samples);
-				iterations,
+		UINFO("Cloud fitness =           %f", fitness);
-				fitness,
+
-				maxDepth,
+		odom = new rtabmap::OdometryICP(decimation, voxel, samples, distance, iterations, fitness);
-				linearUpdate,
-				angularUpdate,
-				resetCountdown);
 	}
 	rtabmap::OdometryThread odomThread(odom);
-	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0);
+	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
 	UEventsManager::addHandler(&odomThread);
 	UEventsManager::addHandler(&odomViewer);