Added parameter KpMaxDepth to limit the features extracted under a specified maximum depth.

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

corelib/include/rtabmap/core/Features2d.h

@@ -30,6 +30,17 @@
 
 namespace rtabmap {
 
+void RTABMAP_EXP filterKeypointsByDepth(
+			std::vector<cv::KeyPoint> & keypoints,
+			const cv::Mat & depth,
+			float depthConstant,
+			float maxDepth);
+void RTABMAP_EXP filterKeypointsByDepth(
+		std::vector<cv::KeyPoint> & keypoints,
+		cv::Mat & descriptors,
+		const cv::Mat & depth,
+		float depthConstant,
+		float maxDepth);
 
 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);

corelib/include/rtabmap/core/Memory.h

@@ -139,6 +139,8 @@ public:
 	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
 	void extractKeypointsAndDescriptors(
 			const cv::Mat & image,
+			const cv::Mat & depth,
+			float depthConstant,
 			std::vector<cv::KeyPoint> & keypoints,
 			cv::Mat & descriptors);
 
@@ -218,6 +220,7 @@ private:
 	float _badSignRatio;;
 	bool _tfIdfLikelihoodUsed;
 	bool _parallelized;
+	float _wordsMaxDepth; // 0=inf
 	int _wordsPerImageTarget; // <0=none, 0=inf
 	std::vector<float> _roiRatios; // size 4
 

corelib/include/rtabmap/core/Parameters.h

@@ -155,6 +155,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Kp, PublishKeypoints,      bool, true, 		"Publishing keypoints.");
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1, 	 		"Naive 0, kdForest 1.");
 	RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, 		"");
+	RTABMAP_PARAM(Kp, 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.");

corelib/include/rtabmap/core/util3d.h

@@ -90,6 +90,11 @@ std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
+pcl::PointXYZ RTABMAP_EXP getDepth(
+		const cv::Mat & depthImage,
+		int x, int y,
+		float depthConstant);
+
 pcl::PointXYZ RTABMAP_EXP getDepth(const cv::Mat & depthImage,
 					   int x, int y,
 					   float cx, float cy,