added keypoints3 field to Image class

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

corelib/include/rtabmap/core/Features2d.h

@@ -66,8 +66,21 @@ void RTABMAP_EXP filterKeypointsByDepth(
 		float cy,
 		float maxDepth);
 
+void RTABMAP_EXP filterKeypointsByDepth(
+		std::vector<cv::KeyPoint> & keypoints,
+		std::vector<cv::Point3f> & keypoints3,
+		float maxDepth);
+
+void RTABMAP_EXP filterKeypointsByDepth(
+		std::vector<cv::KeyPoint> & keypoints,
+		std::vector<cv::Point3f> & keypoints3,
+		cv::Mat & descriptors,
+		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);
+void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, int maxKeypoints);
+void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, cv::Mat & descriptors, int maxKeypoints);
 
 cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
 

corelib/include/rtabmap/core/Image.h

@@ -47,19 +47,7 @@ public:
 		  int id = 0,
 		  const cv::Mat & descriptors = cv::Mat(),
 		  Feature2D::Type featureType = Feature2D::kFeatureUndef,
-		  const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>()) :
+		  const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>());
-		_image(image),
-		_id(id),
-		_descriptors(descriptors),
-		_featureType(featureType),
-		_keypoints(keypoints),
-		_fx(0.0f),
-		_fy(0.0f),
-		_cx(0.0f),
-		_cy(0.0f),
-		_localTransform(Transform::getIdentity())
-	{
-	}
 
 	// Metric constructor
 	Image(const cv::Mat & image,
@@ -70,19 +58,7 @@ public:
 		  float cy,
 		  const Transform & pose,
 		  const Transform & localTransform,
-		  int id = 0) :
+		  int id = 0);
-		_image(image),
-		_id(id),
-		_featureType(Feature2D::kFeatureUndef),
-		_depth(depth),
-		_fx(fx),
-		_fy(fy),
-		_cx(cx),
-		_cy(cy),
-		_pose(pose),
-		_localTransform(localTransform)
-	{
-	}
 
 	// Metric constructor + 2d depth
 	Image(const cv::Mat & image,
@@ -94,20 +70,7 @@ public:
 		  float cy,
 		  const Transform & pose,
 		  const Transform & localTransform,
-		  int id = 0) :
+		  int id = 0);
-		_image(image),
-		_id(id),
-		_featureType(Feature2D::kFeatureUndef),
-		_depth(depth),
-		_depth2d(depth2d),
-		_fx(fx),
-		_fy(fy),
-		_cx(cx),
-		_cy(cy),
-		_pose(pose),
-		_localTransform(localTransform)
-	{
-	}
 
 	virtual ~Image() {}
 
@@ -117,8 +80,9 @@ public:
 	const cv::Mat & descriptors() const {return _descriptors;}
 	Feature2D::Type featureType() const {return _featureType;}
 	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
+	const std::vector<cv::Point3f> & keypoints3() const {return _keypoints3;}
 	void setDescriptors(const cv::Mat & descriptors, Feature2D::Type featureType) {_descriptors = descriptors; _featureType=featureType;}
-	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
+	void setKeypoints(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> * keypoints3 = 0);
 
 	bool isMetric() const {return !_depth.empty() || _fx != 0.0f || _fy != 0.0f || !_pose.isNull();}
 	void setPose(const Transform & pose) {_pose = pose;}
@@ -137,6 +101,7 @@ private:
 	cv::Mat _descriptors;
 	Feature2D::Type _featureType;
 	std::vector<cv::KeyPoint> _keypoints;
+	std::vector<cv::Point3f> _keypoints3;
 
 	// Metric stuff
 	cv::Mat _depth;

corelib/src/CMakeLists.txt

@@ -26,6 +26,7 @@ SET(SRC_FILES
 	
 	util3d.cpp
 	Odometry.cpp
+	Image.cpp
 	
 	toro3d/posegraph3.cpp
 	toro3d/treeoptimizer3_iteration.cpp

corelib/src/Features2d.cpp

@@ -109,6 +109,66 @@ void filterKeypointsByDepth(
 	}
 }
 
+void filterKeypointsByDepth(
+		std::vector<cv::KeyPoint> & keypoints,
+		std::vector<cv::Point3f> & keypoints3,
+		float maxDepth)
+{
+	cv::Mat descriptors;
+	filterKeypointsByDepth(keypoints, keypoints3, descriptors, maxDepth);
+}
+
+void filterKeypointsByDepth(
+		std::vector<cv::KeyPoint> & keypoints,
+		std::vector<cv::Point3f> & keypoints3,
+		cv::Mat & descriptors,
+		float maxDepth)
+{
+	UASSERT(keypoints.size() == keypoints3.size());
+	UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
+	if(keypoints.size())
+	{
+		std::vector<cv::KeyPoint> output(keypoints.size());
+		std::vector<cv::Point3f> output3(keypoints3.size());
+		std::vector<int> indexes(keypoints.size(), 0);
+		int oi=0;
+		for(unsigned int i=0; i<keypoints.size(); ++i)
+		{
+			if(uIsFinite(keypoints3[i].z) && keypoints3[i].z < maxDepth)
+			{
+				output3[oi] = keypoints3[i];
+				output[oi++] = keypoints[i];
+				indexes[i] = 1;
+			}
+		}
+		output.resize(oi);
+		output3.resize(oi);
+		keypoints = output;
+		keypoints3 = output3;
+
+		if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
+		{
+			if(keypoints.size() == 0)
+			{
+				descriptors = cv::Mat();
+			}
+			else
+			{
+				cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
+				int di = 0;
+				for(unsigned int i=0; i<indexes.size(); ++i)
+				{
+					if(indexes[i] == 1)
+					{
+						memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
+					}
+				}
+				descriptors = newDescriptors;
+			}
+		}
+	}
+}
+
 void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
 {
 	cv::Mat descriptors;
@@ -159,6 +219,59 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
 	}
 }
 
+void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, int maxKeypoints)
+{
+	cv::Mat descriptors;
+	limitKeypoints(keypoints, keypoints3, descriptors, maxKeypoints);
+}
+
+void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> keypoints3, cv::Mat & descriptors, int maxKeypoints)
+{
+	UASSERT(((int)keypoints.size() == descriptors.rows && keypoints.size() == keypoints3.size()) || descriptors.rows == 0);
+	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
+	{
+		UTimer timer;
+		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+		// Remove words under the new hessian threshold
+
+		// Sort words by hessian
+		std::multimap<float, int> hessianMap; // <hessian,id>
+		for(unsigned int i = 0; i <keypoints.size(); ++i)
+		{
+			//Keep track of the data, to be easier to manage the data in the next step
+			hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
+		}
+
+		// Remove them from the signature
+		int removed = hessianMap.size()-maxKeypoints;
+		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
+		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
+		std::vector<cv::Point3f> kpts3Tmp(maxKeypoints);
+		cv::Mat descriptorsTmp;
+		if(descriptors.rows)
+		{
+			descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
+		}
+		for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
+		{
+			kptsTmp[k] = keypoints[iter->second];
+			kpts3Tmp[k] = keypoints3[iter->second];
+			if(descriptors.rows)
+			{
+				memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
+			}
+		}
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
+		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
+		keypoints = kptsTmp;
+		keypoints3 = kpts3Tmp;
+		if(descriptors.rows)
+		{
+			descriptors = descriptorsTmp;
+		}
+	}
+}
+
 cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
 	if(!image.empty() && roiRatios.size() == 4)

corelib/src/Image.cpp

@@ -0,0 +1,118 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "rtabmap/core/Image.h"
+
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap
+{
+
+/**
+ * An id is automatically generated if id=0.
+ */
+Image::Image(const cv::Mat & image,
+	  int id,
+	  const cv::Mat & descriptors,
+	  Feature2D::Type featureType,
+	  const std::vector<cv::KeyPoint> & keypoints) :
+	_image(image),
+	_id(id),
+	_descriptors(descriptors),
+	_featureType(featureType),
+	_keypoints(keypoints),
+	_fx(0.0f),
+	_fy(0.0f),
+	_cx(0.0f),
+	_cy(0.0f),
+	_localTransform(Transform::getIdentity())
+{
+}
+
+	// Metric constructor
+Image::Image(const cv::Mat & image,
+		  const cv::Mat & depth,
+		  float fx,
+		  float fy,
+		  float cx,
+		  float cy,
+		  const Transform & pose,
+		  const Transform & localTransform,
+		  int id) :
+	_image(image),
+	_id(id),
+	_featureType(Feature2D::kFeatureUndef),
+	_depth(depth),
+	_fx(fx),
+	_fy(fy),
+	_cx(cx),
+	_cy(cy),
+	_pose(pose),
+	_localTransform(localTransform)
+{
+}
+
+	// Metric constructor + 2d depth
+Image::Image(const cv::Mat & image,
+		  const cv::Mat & depth,
+		  const cv::Mat & depth2d,
+		  float fx,
+		  float fy,
+		  float cx,
+		  float cy,
+		  const Transform & pose,
+		  const Transform & localTransform,
+		  int id) :
+	_image(image),
+	_id(id),
+	_featureType(Feature2D::kFeatureUndef),
+	_depth(depth),
+	_depth2d(depth2d),
+	_fx(fx),
+	_fy(fy),
+	_cx(cx),
+	_cy(cy),
+	_pose(pose),
+	_localTransform(localTransform)
+{
+}
+
+void Image::setKeypoints(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const std::vector<cv::Point3f> * keypoints3)
+{
+	_keypoints = keypoints;
+	if(keypoints3 && keypoints3->size())
+	{
+		UASSERT(_keypoints.size() == keypoints3->size());
+		_keypoints3 = *keypoints3;
+	}
+}
+
+} // namespace rtabmap
+

corelib/src/Memory.cpp

@@ -2906,6 +2906,7 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 	UTimer timer;
 	timer.start();
 	std::vector<cv::KeyPoint> keypoints;
+	std::vector<cv::Point3f> keypoints3;
 	cv::Mat descriptors;
 	int id = image.id();
 	if(_generateIds)
@@ -2958,9 +2959,18 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 			UASSERT(image.descriptors().type() == CV_32F || image.descriptors().type() == CV_8U);
 			descriptors = image.descriptors();
 			keypoints = image.keypoints();
+			keypoints3 = image.keypoints3();
+		}
+		if(keypoints3.size())
+		{
+			filterKeypointsByDepth(keypoints, keypoints3, descriptors, _wordsMaxDepth);
+			limitKeypoints(keypoints, keypoints3, descriptors, _wordsPerImageTarget);
+		}
+		else
+		{
+			filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
+			limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
 		}
-		filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), _wordsMaxDepth);
-		limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
 	}
 	else
 	{
@@ -3011,30 +3021,24 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
 	}
 
 	std::multimap<int, cv::KeyPoint> words;
+	std::multimap<int, pcl::PointXYZ> words3;
 	if(wordIds.size() > 0)
 	{
-		std::vector<cv::KeyPoint>::iterator kpIter = keypoints.begin();
+		UASSERT(wordIds.size() == keypoints.size());
-		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
+		unsigned int i=0;
+		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
-			if(kpIter != keypoints.end())
+			words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
+			if(i < keypoints3.size())
 			{
-				words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
+				words3.insert(std::pair<int, pcl::PointXYZ>(*iter, pcl::PointXYZ(keypoints3[i].x, keypoints3[i].y, keypoints3[i].z)));
-				++kpIter;
-			}
-			else
-			{
-				if(keypoints.size())
-				{
-					UWARN("Words (%d) and keypoints(%d) are not the same size ?!?", (int)wordIds.size(), (int)keypoints.size());
-				}
-				words.insert(std::pair<int, cv::KeyPoint >(*iter, cv::KeyPoint()));
 			}
 		}
 	}
+	UASSERT(keypoints3.size() == 0 || words3.size() == words.size());
 
 	//3d words
-	std::multimap<int, pcl::PointXYZ> words3;
+	if(words3.size() == 0 && !image.depth().empty() && image.depthFx() && image.depthFy())
-	if(!image.depth().empty() && image.depthFx() && image.depthFy())
 	{
 		words3 = util3d::generateWords3(words, image.depth(), image.depthFx(), image.depthFy(), image.depthCx(), image.depthCy(), image.localTransform());
 	}