Added GFTT detector which can be used for Odometry

Modified how depth is computed: Now the mean of neighbors is used as the value instead of interpolation... averaging the Kinect noise.

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

corelib/include/rtabmap/core/Features2d.h

@@ -74,7 +74,14 @@ cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float>
 // Feature2D
 class RTABMAP_EXP Feature2D {
 public:
-	enum Type {kFeatureUndef=-1, kFeatureSurf=0, kFeatureSift=1, kFeatureOrb=2, kFeatureFastFreak=3, kFeatureFastBrief=4};
+	enum Type {kFeatureUndef=-1,
+		kFeatureSurf=0,
+		kFeatureSift=1,
+		kFeatureOrb=2,
+		kFeatureFastFreak=3,
+		kFeatureFastBrief=4,
+		kFeatureGfttFreak=5,
+		kFeatureGfttBrief=6};
 
 public:
 	virtual ~Feature2D() {}
@@ -239,6 +246,71 @@ private:
 	cv::FREAK * _freak;
 };
 
+//GFTT
+class RTABMAP_EXP GFTT : public Feature2D
+{
+public:
+	GFTT(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const;
+
+private:
+	int _maxCorners;
+	double _qualityLevel;
+	double _minDistance;
+	int _blockSize;
+	bool _useHarrisDetector;
+	double _k;
+
+	cv::GFTTDetector * _gftt;
+};
+
+//GFTT_BRIEF
+class RTABMAP_EXP GFTT_BRIEF : public GFTT
+{
+public:
+	GFTT_BRIEF(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_BRIEF();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttBrief;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	int bytes_;
+
+	cv::BriefDescriptorExtractor * _brief;
+};
+
+//GFTT_FREAK
+class RTABMAP_EXP GFTT_FREAK : public GFTT
+{
+public:
+	GFTT_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttFreak;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+	cv::FREAK * _freak;
+};
+
+
 }
 
 #endif /* KEYPOINTDESCRIPTOR_H_ */

corelib/include/rtabmap/core/Odometry.h

@@ -101,13 +101,16 @@ public:
 	virtual ~OdometryBOW();
 
 	virtual void reset();
+	const std::multimap<int, std::pair<int, pcl::PointXYZ> > & getLocalMap() const {return localMap_;}
+	std::multimap<int,pcl::PointXYZ> getLocalMeansMap() const;
+	const Memory * getMemory() const {return _memory;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, int * quality = 0);
 
 private:
 	Memory * _memory;
-	std::multimap<int, pcl::PointXYZ> localMap_;
+	std::multimap<int, std::pair<int, pcl::PointXYZ> > localMap_;
 };
 
 class RTABMAP_EXP OdometryICP : public Odometry

corelib/include/rtabmap/core/Parameters.h

@@ -167,7 +167,7 @@ class RTABMAP_EXP Parameters
 	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, 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, DetectorStrategy,      int, 0, 			"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF.");
+	RTABMAP_PARAM(Kp, DetectorStrategy,      int, 0, 			"0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/BRIEF 6=GFTT/BRIEF.");
 	RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,   bool, false, 		"Use of the td-idf strategy to compute the likelihood.");
 	RTABMAP_PARAM(Kp, Parallelized,          bool, true, 		"If the dictionary update and signature creation were parallelized.");
 	RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", 		"Region of interest ratios [left, right, top, bottom].");
@@ -202,6 +202,13 @@ class RTABMAP_EXP Parameters
 	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(GFTT, MaxCorners, int, 1000, "");
+	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
+	RTABMAP_PARAM(GFTT, MinDistance, double, 1, "");
+	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
+	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
+	RTABMAP_PARAM(GFTT, K, double, 0.04, "");
+
 	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).");

corelib/include/rtabmap/core/util3d.h

@@ -121,7 +121,8 @@ pcl::PointXYZ RTABMAP_EXP getDepth(
 		float x, float y,
 		float cx, float cy,
 		float fx, float fy,
-		bool interpolate);
+		bool smoothing,
+		float maxZError = 0.03f);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,