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,

corelib/src/Features2d.cpp

@@ -705,4 +705,141 @@ cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 	return descriptors;
 }
 
+//////////////////////////
+//GFTT
+//////////////////////////
+GFTT::GFTT(const ParametersMap & parameters) :
+		_maxCorners(Parameters::defaultGFTTMaxCorners()),
+		_qualityLevel(Parameters::defaultGFTTQualityLevel()),
+		_minDistance(Parameters::defaultGFTTMinDistance()),
+		_blockSize(Parameters::defaultGFTTBlockSize()),
+		_useHarrisDetector(Parameters::defaultGFTTUseHarrisDetector()),
+		_k(Parameters::defaultGFTTK()),
+		_gftt(0)
+{
+	parseParameters(parameters);
+}
+
+GFTT::~GFTT()
+{
+	if(_gftt)
+	{
+		delete _gftt;
+	}
+}
+
+void GFTT::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kGFTTMaxCorners(), _maxCorners);
+	Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
+	Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
+	Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
+	Parameters::parse(parameters, Parameters::kGFTTUseHarrisDetector(), _useHarrisDetector);
+	Parameters::parse(parameters, Parameters::kGFTTK(), _k);
+
+	if(_gftt)
+	{
+		delete _gftt;
+		_gftt = 0;
+	}
+	_gftt = new cv::GFTTDetector(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
+}
+
+std::vector<cv::KeyPoint> GFTT::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);
+	_gftt->detect(imgRoi, keypoints); // Opencv keypoints
+	return keypoints;
+}
+
+//////////////////////////
+//FAST-BRIEF
+//////////////////////////
+GFTT_BRIEF::GFTT_BRIEF(const ParametersMap & parameters) :
+	GFTT(parameters),
+	bytes_(Parameters::defaultBRIEFBytes()),
+	_brief(0)
+{
+	parseParameters(parameters);
+}
+
+GFTT_BRIEF::~GFTT_BRIEF()
+{
+	if(_brief)
+	{
+		delete _brief;
+	}
+}
+
+void GFTT_BRIEF::parseParameters(const ParametersMap & parameters)
+{
+	GFTT::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kBRIEFBytes(), bytes_);
+	if(_brief)
+	{
+		delete _brief;
+		_brief = 0;
+	}
+	_brief = new cv::BriefDescriptorExtractor(bytes_);
+}
+
+cv::Mat GFTT_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
+//////////////////////////
+GFTT_FREAK::GFTT_FREAK(const ParametersMap & parameters) :
+	GFTT(parameters),
+	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
+	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
+	patternScale_(Parameters::defaultFREAKPatternScale()),
+	nOctaves_(Parameters::defaultFREAKNOctaves()),
+	_freak(0)
+{
+	parseParameters(parameters);
+}
+
+GFTT_FREAK::~GFTT_FREAK()
+{
+	if(_freak)
+	{
+		delete _freak;
+	}
+}
+
+void GFTT_FREAK::parseParameters(const ParametersMap & parameters)
+{
+	GFTT::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
+	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
+	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
+
+	if(_freak)
+	{
+		delete _freak;
+		_freak = 0;
+	}
+
+	_freak = new cv::FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
+}
+
+cv::Mat GFTT_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

@@ -405,6 +405,14 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			_feature2D = new ORB(parameters);
 			_featureType = Feature2D::kFeatureOrb;
 			break;
+		case Feature2D::kFeatureGfttFreak:
+			_feature2D = new GFTT_FREAK(parameters);
+			_featureType = Feature2D::kFeatureGfttFreak;
+			break;
+		case Feature2D::kFeatureGfttBrief:
+			_feature2D = new GFTT_BRIEF(parameters);
+			_featureType = Feature2D::kFeatureGfttBrief;
+			break;
 		case Feature2D::kFeatureSurf:
 		default:
 			_feature2D = new SURF(parameters);

corelib/src/Odometry.cpp

@@ -88,9 +88,15 @@ void Odometry::reset()
 
 bool Odometry::isLargeEnoughTransform(const Transform & transform)
 {
-	return fabs(transform.x()) > _linearUpdate ||
+	float x,y,z, roll,pitch,yaw;
-		   fabs(transform.y()) > _linearUpdate ||
+	transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-	       fabs(transform.z()) > _linearUpdate;
+	return (_linearUpdate == 0.0f && _angularUpdate == 0.0f) ||
+			fabs(x) > _linearUpdate ||
+		   fabs(y) > _linearUpdate ||
+	       fabs(z) > _linearUpdate ||
+	       fabs(roll) > _angularUpdate ||
+			fabs(pitch) > _angularUpdate ||
+			fabs(yaw) > _angularUpdate;
 }
 
 Transform Odometry::process(SensorData & data, int * quality)
@@ -147,7 +153,8 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 			group.compare("BRIEF") == 0 ||
 			group.compare("FAST") == 0 ||
 			group.compare("ORB") == 0 ||
-			group.compare("FREAK") == 0)
+			group.compare("FREAK") == 0 ||
+			group.compare("GFTT") == 0)
 		{
 			customParameters.insert(*iter);
 		}
@@ -173,6 +180,34 @@ void OdometryBOW::reset()
 	localMap_.clear();
 }
 
+std::multimap<int,pcl::PointXYZ> OdometryBOW::getLocalMeansMap() const
+{
+	std::multimap<int,pcl::PointXYZ> localMeansMap;
+	for(std::multimap<int, std::pair<int, pcl::PointXYZ> >::const_iterator iter=localMap_.begin();
+		iter!= localMap_.end();)
+	{
+		int id = iter->first;
+		pcl::PointXYZ sumPt = iter->second.second;
+		int count = 1;
+		++iter;
+		std::multimap<int, std::pair<int, pcl::PointXYZ> >::const_iterator jter=iter;
+		while(jter->first == id && jter!= localMap_.end())
+		{
+			sumPt.x += jter->second.second.x;
+			sumPt.y += jter->second.second.y;
+			sumPt.z += jter->second.second.z;
+			++count;
+			++jter;
+		}
+		iter = jter;
+
+		sumPt.x /= float(count);
+		sumPt.y /= float(count);
+		sumPt.z /= float(count);
+		localMeansMap.insert(std::make_pair(id, sumPt));
+	}
+	return localMeansMap;
+}
 
 // return not null transform if odometry is correctly computed
 Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
@@ -207,11 +242,14 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
 
+				//Create the local map with mean of all features
+				std::multimap<int, pcl::PointXYZ> localMeansMap = getLocalMeansMap();
+
 				// No need to set max depth here, it is already applied in extractKeypointsAndDescriptors() above.
 				// Also! the localMap_ have points not in camera frame anymore (in local map frame), so filtering
 				// by depth here is wrong!
 				util3d::findCorrespondences(
-						localMap_,
+						localMeansMap,
 						newSignature->getWords3(),
 						*inliers1,
 						*inliers2,
@@ -274,7 +312,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
 								if(pcl::isFinite(pt))
 								{
 									pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
-									localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
+									localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt2)));
 								}
 							}
 						}
@@ -290,25 +328,35 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
 						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 &&
+							if(newSignature->getWords3().count(*iter) == 1)
-							   uniqueCorrespondences.find(*iter) == uniqueCorrespondences.end())
 							{
 								const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
 								if(pcl::isFinite(pt))
 								{
 									pcl::PointXYZ pt2 = util3d::transformPoint(pt, transform);
-									localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt2));
+									localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt2)));
 								}
 							}
 						}
-						while(localMap_.size() && (int)localMap_.size() > this->getLocalHistory() && _memory->getStMem().size()>1)
+						while(localMap_.size() && (int)uUniqueKeys(localMap_).size() > this->getLocalHistory() && _memory->getStMem().size()>1)
 						{
-							std::list<int> deletedWords;
+							int nodeId = *_memory->getStMem().begin();
-							_memory->deleteLocation(*_memory->getStMem().begin(), &deletedWords);
+							std::list<int> removedPts = uUniqueKeys(_memory->getSignature(nodeId)->getWords3());
-							for(std::list<int>::iterator iter = deletedWords.begin(); iter!=deletedWords.end(); ++iter)
+							for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
 							{
-								localMap_.erase(*iter);
+								bool removed = false;
+								for(std::multimap<int, std::pair<int, pcl::PointXYZ> >::iterator jter=localMap_.lower_bound(*iter);
+									jter->first == *iter && !removed;
+									++jter)
+								{
+									if(jter->second.first == nodeId)
+									{
+										localMap_.erase(jter);
+										removed = true;
+									}
+								}
 							}
+							_memory->deleteLocation(*_memory->getStMem().begin());
 						}
 					}
 					else
@@ -331,7 +379,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
 					const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
 					if(pcl::isFinite(pt))
 					{
-						localMap_.insert(std::make_pair<int, pcl::PointXYZ>(*iter, pt));
+						localMap_.insert(std::make_pair(*iter, std::make_pair(newSignature->id(), pt)));
 					}
 				}
 			}
@@ -340,11 +388,13 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality)
 		_memory->emptyTrash();
 	}
 
-	UINFO("Odom update time = %fs features=%d inliers=%d/%d dict=%d nodes=%d",
+	UINFO("Odom update time = %fs features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
 			timer.elapsed(),
 			nFeatures,
 			inliers,
 			correspondences,
+			(int)uUniqueKeys(localMap_).size(),
+			(int)localMap_.size(),
 			(int)_memory->getVWDictionary()->getVisualWords().size(),
 			(int)_memory->getStMem().size());
 	return output;

corelib/src/util3d.cpp

@@ -440,7 +440,8 @@ pcl::PointXYZ getDepth(
 		float x, float y,
 		float cx, float cy,
 		float fx, float fy,
-		bool interpolate)
+		bool smoothing,
+		float maxZError)
 {
 	pcl::PointXYZ pt;
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();
@@ -453,104 +454,73 @@ pcl::PointXYZ getDepth(
 		return pt;
 	}
 
-	// Use correct principal point from calibration
-	float center_x = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
-	float center_y = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
-
 	bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
 
-	// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+	// Inspired from RGBDFrame::getGaussianMixtureDistribution() method from
-	float unit_scaling = isInMM?0.001f:1.0f;
+	// https://github.com/ccny-ros-pkg/rgbdtools/blob/master/src/rgbd_frame.cpp
-	float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
+	// Window weights:
-	float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
+	//  | 1 | 2 | 1 |
+	//  | 2 | 4 | 2 |
+	//  | 1 | 2 | 1 |
+	int u = int(x+0.5f);
+	int v = int(y+0.5f);
+	int u_start = std::max(u-1, 0);
+	int v_start = std::max(v-1, 0);
+	int u_end = std::min(u+1, depthImage.cols-1);
+	int v_end = std::min(v+1, depthImage.rows-1);
 
-	float depth = 0.0f;
+	float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
-	if(!interpolate || (int(x) < 1 || int(y) < 1 || int(x) >= depthImage.cols-1 || int(y) >= depthImage.rows-1))
+	if(depth!=0.0f && uIsFinite(depth))
 	{
-		if(interpolate)
+		if(smoothing)
 		{
-			UERROR("Cannot interpolate for points on the image side. Falling back to no interpolation.");
+			float sumWeights = 0.0f;
+			float sumDepths = 0.0f;
+			for(int uu = u_start; uu <= u_end; ++uu)
+			{
+				for(int vv = v_start; vv <= v_end; ++vv)
+				{
+					if(!(uu == u && vv == v))
+					{
+						float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
+						// ignore if not valid or depth difference is too high
+						if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
+						{
+							if(uu == u || vv == v)
+							{
+								sumWeights+=2.0f;
+								d*=2.0f;
+							}
+							else
+							{
+								sumWeights+=1.0f;
+							}
+							sumDepths += d;
+						}
+					}
+				}
+			}
+			// set window weight to center point
+			depth *= 4.0f;
+			sumWeights += 4.0f;
+
+			// mean
+			depth = (depth+sumDepths)/sumWeights;
 		}
 
-		// select directly to corresponding pixel
+		// Use correct principal point from calibration
-		depth = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
+		cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
+		cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
+
+		// Fill in XYZ
+		pt.x = (x - cx) * depth / fx;
+		pt.y = (y - cy) * depth / fy;
+		pt.z = depth;
 	}
 	else
-	{
-		// Interpolate x axis
-		float depthX = 0.0f;
-		float first;
-		float second;
-		if(int(x) == int(x+0.5f))
-		{
-			first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)-1):depthImage.at<float>(int(y),int(x)-1);
-			second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
-		}
-		else
-		{
-			first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
-			second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)+1):depthImage.at<float>(int(y),int(x)+1);
-		}
-
-		if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
-		{
-			// y = ax + b...
-			float a = second-first;
-			float b = first - a*(float(int(x))-0.5f);
-			depthX = a*(x) + b;
-			//UDEBUG("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthX);
-		}
-
-		if(depthX != 0.0f)
-		{
-			depth = depthX;
-		}
-		else
-		{
-			// Interpolate y axis
-			float depthY = 0.0f;
-			if(int(y) == int(y+0.5f))
-			{
-				first = isInMM?(float)depthImage.at<uint16_t>(int(y)-1,int(x)):depthImage.at<float>(int(y)-1,int(x));
-				second = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
-			}
-			else
-			{
-				first = isInMM?(float)depthImage.at<uint16_t>(int(y),int(x)):depthImage.at<float>(int(y),int(x));
-				second = isInMM?(float)depthImage.at<uint16_t>(int(y)+1,int(x)):depthImage.at<float>(int(y)+1,int(x));
-			}
-
-			if(first != 0.0f && uIsFinite(first) && second != 0.0f && uIsFinite(second))
-			{
-				// y = ax + b...
-				float a = second-first;
-				float b = first - a*(float(int(y))-0.5f);
-				depthY = a*(y) + b;
-				//UWARN("x=%f, y=%f, first=%f, second=%f, a=%f, b=%f, depth=%f", x,y, first,second, a,b, depthY);
-			}
-			if(depthY != 0.0f)
-			{
-				depth = depthY;
-			}
-			else
-			{
-				//UWARN("Could not compute depth for x=%f, y=%f", x,y);
-			}
-		}
-	}
-
-	// Check for invalid measurements
-	if (depth==0.0f || !uIsFinite(depth))
 	{
 		pt.x = pt.y = pt.z = bad_point;
 	}
-	else
-	{
-		// Fill in XYZ
-		pt.x = (x - center_x) * depth * constant_x;
-		pt.y = (y - center_y) * depth * constant_y;
-		pt.z = depth*unit_scaling;
-	}
 	return pt;
 }
 

guilib/src/PreferencesDialog.cpp

@@ -382,6 +382,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_FREAKPatternScale->setObjectName(Parameters::kFREAKPatternScale().c_str());
 	_ui->spinBox_FREAKNOctaves->setObjectName(Parameters::kFREAKNOctaves().c_str());
 
+	//GFTT detector
+	_ui->spinBox_GFTT_maxCorners->setObjectName(Parameters::kGFTTMaxCorners().c_str());
+	_ui->doubleSpinBox_GFTT_qualityLevel->setObjectName(Parameters::kGFTTQualityLevel().c_str());
+	_ui->doubleSpinBox_GFTT_minDistance->setObjectName(Parameters::kGFTTMinDistance().c_str());
+	_ui->spinBox_GFTT_blockSize->setObjectName(Parameters::kGFTTBlockSize().c_str());
+	_ui->checkBox_GFTT_useHarrisDetector->setObjectName(Parameters::kGFTTUseHarrisDetector().c_str());
+	_ui->doubleSpinBox_GFTT_k->setObjectName(Parameters::kGFTTK().c_str());
+
 	// verifyHypotheses
 	_ui->comboBox_vh_strategy->setObjectName(Parameters::kRtabmapVhStrategy().c_str());
 	_ui->surf_spinBox_matchCountMinAccepted->setObjectName(Parameters::kVhEpMatchCountMin().c_str());
@@ -1902,6 +1910,16 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
 						this->addParameters(_ui->groupBox_detector_fast2);
 						this->addParameters(_ui->groupBox_detector_brief2);
 					}
+					else if(value == 5) //  gftt+freak
+					{
+						this->addParameters(_ui->groupBox_detector_gftt2);
+						this->addParameters(_ui->groupBox_detector_freak2);
+					}
+					else if(value == 6) //  gftt+brief
+					{
+						this->addParameters(_ui->groupBox_detector_gftt2);
+						this->addParameters(_ui->groupBox_detector_brief2);
+					}
 				}
 				else if(comboBox == _ui->globalDetection_icpType)
 				{

guilib/src/ui/preferencesDialog.ui

@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>736</width>
+            <width>737</width>
-            <height>751</height>
+            <height>895</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>0</number>
+                <number>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4226,6 +4226,155 @@ When set to false, no new words are added to dictionary, so no more updates are
                  </item>
                 </layout>
                </widget>
+               <widget class="QWidget" name="page_16">
+                <layout class="QVBoxLayout" name="verticalLayout_9">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_detector_gftt2">
+                   <property name="title">
+                    <string>GFTT</string>
+                   </property>
+                   <layout class="QGridLayout" name="gridLayout_21" columnstretch="0,1">
+                    <item row="1" column="0">
+                     <widget class="QDoubleSpinBox" name="doubleSpinBox_GFTT_qualityLevel">
+                      <property name="maximum">
+                       <double>1.000000000000000</double>
+                      </property>
+                      <property name="singleStep">
+                       <double>0.010000000000000</double>
+                      </property>
+                      <property name="value">
+                       <double>0.010000000000000</double>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="0">
+                     <widget class="QDoubleSpinBox" name="doubleSpinBox_GFTT_minDistance">
+                      <property name="decimals">
+                       <number>1</number>
+                      </property>
+                      <property name="value">
+                       <double>1.000000000000000</double>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="4" column="0">
+                     <widget class="QCheckBox" name="checkBox_GFTT_useHarrisDetector">
+                      <property name="text">
+                       <string/>
+                      </property>
+                      <property name="checked">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="4" column="1">
+                     <widget class="QLabel" name="label_172">
+                      <property name="text">
+                       <string>Use Harris detector.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="5" column="0">
+                     <widget class="QDoubleSpinBox" name="doubleSpinBox_GFTT_k">
+                      <property name="maximum">
+                       <double>1.000000000000000</double>
+                      </property>
+                      <property name="singleStep">
+                       <double>0.010000000000000</double>
+                      </property>
+                      <property name="value">
+                       <double>0.040000000000000</double>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="5" column="1">
+                     <widget class="QLabel" name="label_173">
+                      <property name="text">
+                       <string>K.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="3" column="0">
+                     <widget class="QSpinBox" name="spinBox_GFTT_blockSize">
+                      <property name="value">
+                       <number>3</number>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="0" column="0">
+                     <widget class="QSpinBox" name="spinBox_GFTT_maxCorners">
+                      <property name="maximum">
+                       <number>100000</number>
+                      </property>
+                      <property name="value">
+                       <number>1000</number>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="0" column="1">
+                     <widget class="QLabel" name="label_174">
+                      <property name="text">
+                       <string>Maximum corners.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="1" column="1">
+                     <widget class="QLabel" name="label_175">
+                      <property name="text">
+                       <string>Quality level.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="2" column="1">
+                     <widget class="QLabel" name="label_176">
+                      <property name="text">
+                       <string>Mininum distance.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="3" column="1">
+                     <widget class="QLabel" name="label_177">
+                      <property name="text">
+                       <string>Block size.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_25">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>644</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
                <widget class="QWidget" name="page_19">
                 <layout class="QVBoxLayout" name="verticalLayout_13">
                  <item>
@@ -5635,6 +5784,16 @@ Warning when set to false: when some nodes are transferred, the first referentia
                           <string>FAST+BRIEF</string>
                          </property>
                         </item>
+                        <item>
+                         <property name="text">
+                          <string>GFTT+FREAK</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>GFTT+BRIEF</string>
+                         </property>
+                        </item>
                        </widget>
                       </item>
                       <item row="13" column="1">

tools/OdometryViewer/main.cpp

@@ -42,7 +42,7 @@ void showUsage()
 	printf("\nUsage:\n"
 			"odometryViewer [options]\n"
 			"Options:\n"
-			"  -o #                      Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF\n"
+			"  -o #                      Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/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"
@@ -125,7 +125,7 @@ int main (int argc, char * argv[])
 			if(i < argc)
 			{
 				odomType = std::atoi(argv[i]);
-				if(odomType < 0 || odomType > 4)
+				if(odomType < 0 || odomType > 6)
 				{
 					showUsage();
 				}
@@ -577,6 +577,14 @@ int main (int argc, char * argv[])
 	{
 		odomName = "FAST+BRIEF";
 	}
+	else if(odomType == 5)
+	{
+		odomName = "GFTT+FREAK";
+	}
+	else if(odomType == 6)
+	{
+		odomName = "GFTT+BRIEF";
+	}
 
 	if(icp)
 	{
@@ -659,7 +667,7 @@ int main (int argc, char * argv[])
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), uNumber2Str(fastThr)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTGpu(), uBool2Str(gpu)));
 		}
-		if(odomType == 4)
+		if(odomType == 4 || odomType == 6)
 		{
 			UINFO("BRIEF bytes =             %d", briefBytes);
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), uNumber2Str(briefBytes)));