Added a quality odometry threshold, turning the background yellow when the mapping area has too low features

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

corelib/include/rtabmap/core/Odometry.h

@@ -34,7 +34,7 @@ class RTABMAP_EXP Odometry
 {
 public:
 	virtual ~Odometry() {}
-	Transform process(Image & image);
+	Transform process(Image & image, int * quality = 0);
 	virtual void reset();
 
 	bool isLargeEnoughTransform(const Transform & transform);
@@ -44,18 +44,20 @@ public:
 	int getMinInliers() const {return _minInliers;}
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
+	float getWordsRatio() const {return _wordsRatio;}
 	float getMaxDepth() const {return _maxDepth;}
 	float geLinearUpdate() const {return _linearUpdate;}
 	float getAngularUpdate() const {return _angularUpdate;}
 
 private:
-	virtual Transform computeTransform(Image & image) = 0;
+	virtual Transform computeTransform(Image & image, int * quality = 0) = 0;
 
 private:
 	int _maxFeatures;
 	int _minInliers;
 	float _inlierDistance;
 	int _iterations;
+	float _wordsRatio;
 	float _maxDepth;
 	float _linearUpdate;
 	float _angularUpdate;
@@ -68,6 +70,7 @@ protected:
 			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(),
@@ -83,6 +86,7 @@ public:
 			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(),
@@ -96,7 +100,7 @@ public:
 	virtual void reset();
 
 private:
-	virtual Transform computeTransform(Image & image);
+	virtual Transform computeTransform(Image & image, int * quality = 0);
 
 private:
 	int _briefBytes;
@@ -120,6 +124,7 @@ public:
 			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(),
@@ -132,7 +137,7 @@ public:
 	virtual void reset();
 
 private:
-	virtual Transform computeTransform(Image & image);
+	virtual Transform computeTransform(Image & image, int * quality = 0);
 
 private:
 	Memory * _memory;
@@ -156,7 +161,7 @@ public:
 	void reset();
 
 private:
-	virtual Transform computeTransform(Image & image);
+	virtual Transform computeTransform(Image & image, int * quality = 0);
 
 private:
 	int _decimation;

corelib/include/rtabmap/core/OdometryEvent.h

@@ -17,16 +17,19 @@ class OdometryEvent : public UEvent
 {
 public:
 	OdometryEvent(
-		const Image & data) :
-			_data(data) {}
+		const Image & data, int quality = 0) :
+			_data(data),
+			_quality(quality) {}
 	virtual ~OdometryEvent() {}
 	virtual std::string getClassName() const {return "OdometryEvent";}
 
 	bool isValid() const {return !_data.pose().isNull();}
 	const Image & data() const {return _data;}
+	int quality() const {return _quality;}
 
 private:
 	Image _data;
+	int _quality;
 };
 
 class OdometryResetEvent : public UEvent

corelib/include/rtabmap/core/Parameters.h

@@ -224,6 +224,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, MinInliers,             int, 10, 		"Minimum visual word correspondences to compute geometry transform.");
 	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(OdomBin, BriefBytes,         	   int, 32, 	"");

corelib/src/Odometry.cpp

@@ -36,6 +36,7 @@ Odometry::Odometry(
 		int maxWords,
 		int minInliers,
 		int iterations,
+		float wordsRatio,
 		float maxDepth,
 		float linearUpdate,
 		float angularUpdate,
@@ -44,6 +45,7 @@ Odometry::Odometry(
 			_minInliers(minInliers),
 			_inlierDistance(inlierDistance),
 			_iterations(iterations),
+			_wordsRatio(wordsRatio),
 			_maxDepth(maxDepth),
 			_linearUpdate(linearUpdate),
 			_angularUpdate(angularUpdate),
@@ -59,6 +61,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_minInliers(Parameters::defaultOdomMinInliers()),
 		_inlierDistance(Parameters::defaultOdomInlierDistance()),
 		_iterations(Parameters::defaultOdomIterations()),
+		_wordsRatio(Parameters::defaultOdomWordsRatio()),
 		_maxDepth(Parameters::defaultOdomMaxDepth()),
 		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
 		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
@@ -72,6 +75,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
 	Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
 	Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
+	Parameters::parse(parameters, Parameters::kOdomWordsRatio(), _wordsRatio);
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
 }
@@ -89,9 +93,9 @@ bool Odometry::isLargeEnoughTransform(const Transform & transform)
 	       fabs(transform.z()) > _linearUpdate;
 }
 
-Transform Odometry::process(Image & image)
+Transform Odometry::process(Image & image, int * quality)
 {
-	Transform t = this->computeTransform(image);
+	Transform t = this->computeTransform(image, quality);
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
@@ -117,6 +121,7 @@ OdometryBinary::OdometryBinary(
 		int maxWords,
 		int minInliers,
 		int iterations,
+		float wordsRatio,
 		float maxDepth,
 		float linearUpdate,
 		float angularUpdate,
@@ -125,7 +130,7 @@ OdometryBinary::OdometryBinary(
 		int fastThreshold,
 		bool fastNonmaxSuppression,
 		bool bruteForceMatching) :
-	Odometry(inlierDistance, maxWords, minInliers, iterations, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
+	Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
  	_briefBytes(briefBytes),
 	_fastThreshold(fastThreshold),
 	_fastNonmaxSuppression(fastNonmaxSuppression),
@@ -156,8 +161,8 @@ void OdometryBinary::reset()
 }
 
 
-// return true if odometry is correctly computed
-Transform OdometryBinary::computeTransform(Image & image)
+// return not null transform if odometry is correctly computed
+Transform OdometryBinary::computeTransform(Image & image, int * quality)
 {
 	UTimer timer;
 	cv::Mat imageMono;
@@ -187,7 +192,7 @@ Transform OdometryBinary::computeTransform(Image & image)
 
 	if(_lastKeypoints.size())
 	{
-		if(newDescriptors.rows && newDescriptors.rows > (int)_lastKeypoints.size()/2) // at least 50% keypoints
+		if(newDescriptors.rows && newDescriptors.rows > (int)(getWordsRatio() * float(_lastKeypoints.size()))) // at least 50% keypoints
 		{
 			cv::Mat results;
 			cv::Mat dists;
@@ -308,6 +313,11 @@ Transform OdometryBinary::computeTransform(Image & image)
 				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())
 				{
@@ -335,8 +345,8 @@ Transform OdometryBinary::computeTransform(Image & image)
 		}
 		else if(newDescriptors.rows)
 		{
-			UWARN("At least 50%% keypoints of the last image required. New=%d last=%d",
-					newDescriptors.rows, _lastKeypoints.size());
+			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
+					getWordsRatio()*100.0f, newDescriptors.rows, _lastKeypoints.size());
 		}
 		else
 		{
@@ -368,13 +378,14 @@ OdometryBOW::OdometryBOW(
 		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, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
+	Odometry(inlierDistance, maxWords, minInliers, iterations, wordsRatio, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
 	_memory(new Memory())
 {
 	ParametersMap customParameters;
@@ -421,8 +432,8 @@ void OdometryBOW::reset()
 }
 
 
-// return true if odometry is correctly computed
-Transform OdometryBOW::computeTransform(Image & image)
+// return not null transform if odometry is correctly computed
+Transform OdometryBOW::computeTransform(Image & image, int * quality)
 {
 	UTimer timer;
 	Transform output;
@@ -444,10 +455,10 @@ Transform OdometryBOW::computeTransform(Image & image)
 		if(previousSignature && newSignature)
 		{
 			Transform transform;
-			if(newSignature->getWords3().size() < previousSignature->getWords3().size()/2)
+			if(newSignature->getWords3().size() < (unsigned int)(getWordsRatio() * float(previousSignature->getWords3().size())))
 			{
-				UWARN("At least 50%% keypoints of the last image required. New=%d last=%d",
-						newSignature->getWords3().size(), 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())
 			{
@@ -472,6 +483,11 @@ Transform OdometryBOW::computeTransform(Image & image)
 							this->getIterations(),
 							&inliers);
 
+					if(quality)
+					{
+						*quality = inliers;
+					}
+
 					if(inliers < this->getMinInliers())
 					{
 						transform.setNull();
@@ -527,7 +543,7 @@ OdometryICP::OdometryICP(
 		float linearUpdate,
 		float angularUpdate,
 		int resetCoutdown) :
-	Odometry(0, 0, 0, 0, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
+	Odometry(0, 0, 0, 0, 0, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
 	_decimation(decimation),
 	_voxelSize(voxelSize),
 	_samples(samples),
@@ -563,8 +579,8 @@ void OdometryICP::reset()
 	_previousCloud.reset(new pcl::PointCloud<pcl::PointNormal>);
 }
 
-// return not null if odometry is correctly computed
-Transform OdometryICP::computeTransform(Image & image)
+// return not null transform if odometry is correctly computed
+Transform OdometryICP::computeTransform(Image & image, int * quality)
 {
 	UTimer timer;
 	Transform output;
@@ -698,9 +714,10 @@ void OdometryThread::mainLoop()
 	getImage(image);
 	if(!image.empty())
 	{
-		Transform pose = _odometry->process(image);
+		int quality = 0;
+		Transform pose = _odometry->process(image, &quality);
 		image.setPose(pose); // a null pose notify that odometry could not be computed
-		this->post(new OdometryEvent(image));
+		this->post(new OdometryEvent(image, quality));
 	}
 }