rtabmap:

-Fixed map id not incremented on Odometry reset (identity transform was missed)
-Keeping maximum odometry variance between two loop closure updates
Odometry:
-removed parameters Odom/FeaturesRatio, Odom/LinearUpdate, Odom/AngularUpdate
-added parameter Odom/FillInfoData
-expended OdometryInfo class with features stuff
Gui:
-show inliers/outliers features in Odometry view

corelib/include/rtabmap/core/Odometry.h

@@ -60,8 +60,6 @@ public:
 	Transform process(const SensorData & data, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 
-	bool isLargeEnoughTransform(const Transform & transform);
-
 	//getters
 	const Transform & getPose() const {return _pose;}
 	int getMaxFeatures() const  {return _maxFeatures;}
@@ -70,10 +68,8 @@ public:
 	float getInlierDistance() const {return _inlierDistance;}
 	int getIterations() const {return _iterations;}
 	int getRefineIterations() const {return _refineIterations;}
-	float getFeaturesRatio() const {return _featuresRatio;}
 	float getMaxDepth() const {return _maxDepth;}
-	float geLinearUpdate() const {return _linearUpdate;}
+	bool isInfoDataFilled() const {return _fillInfoData;}
-	float getAngularUpdate() const {return _angularUpdate;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
@@ -85,12 +81,10 @@ private:
 	float _inlierDistance;
 	int _iterations;
 	int _refineIterations;
-	float _featuresRatio;
 	float _maxDepth;
-	float _linearUpdate;
-	float _angularUpdate;
 	int _resetCountdown;
 	bool _force2D;
+	bool _fillInfoData;
 	Transform _pose;
 	int _resetCurrentCount;
 

corelib/include/rtabmap/core/OdometryInfo.h

@@ -40,7 +40,8 @@ public:
 		variance(-1),
 		features(-1),
 		localMapSize(-1),
-		time(0.0f)
+		time(-1),
+		type(-1)
 	{}
 	bool lost;
 	int matches;
@@ -49,6 +50,18 @@ public:
 	int features;
 	int localMapSize;
 	float time;
+
+	int type; // 0=BOW, 1=Optical Flow, 2=ICP
+
+	// BOW odometry
+	std::multimap<int, cv::KeyPoint> words;
+	std::vector<int> wordMatches;
+	std::vector<int> wordInliers;
+
+	// Optical Flow odometry
+	std::vector<cv::KeyPoint> refCorners;
+	std::vector<cv::KeyPoint> newCorners;
+	std::vector<int> cornerInliers;
 };
 
 }

corelib/include/rtabmap/core/Parameters.h

@@ -296,8 +296,6 @@ class RTABMAP_EXP Parameters
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");
 	RTABMAP_PARAM(Odom, FeatureType,            int, 6, 	    "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
-	RTABMAP_PARAM(Odom, LinearUpdate,           float, 0.0, 	"Min linear displacement to update odometry.");
-	RTABMAP_PARAM(Odom, AngularUpdate,          float, 0.0, 	"Min angular displacement to update odometry.");
 	RTABMAP_PARAM(Odom, MaxFeatures,            int, 0, 		"0 no limits.");
 	RTABMAP_PARAM(Odom, InlierDistance,         float, 0.02, 	"Maximum distance for visual word correspondences.");
 	RTABMAP_PARAM(Odom, MinInliers,             int, 20, 		"Minimum visual word correspondences to compute geometry transform.");
@@ -306,8 +304,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, MaxDepth,               float, 4.0, 	"Max depth of the words (0 means no limit).");
 	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_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
-	RTABMAP_PARAM(Odom, FeaturesRatio,          float, 0.0, 	"Minimum ratio of keypoints between the current image and the last image to compute odometry.");
 	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
+	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, false,     "Fill info with data (inliers/outliers features).");
 
 	// Odometry Bag-of-words
 	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,      "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");

corelib/include/rtabmap/core/RtabmapThread.h

@@ -109,6 +109,8 @@ private:
 
 	Rtabmap * _rtabmap;
 	bool _paused;
+	Transform lastPose_;
+	float _variance;
 };
 
 } /* namespace rtabmap */

corelib/src/Odometry.cpp

@@ -63,26 +63,22 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_inlierDistance(Parameters::defaultOdomInlierDistance()),
 		_iterations(Parameters::defaultOdomIterations()),
 		_refineIterations(Parameters::defaultOdomRefineIterations()),
-		_featuresRatio(Parameters::defaultOdomFeaturesRatio()),
 		_maxDepth(Parameters::defaultOdomMaxDepth()),
-		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
-		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
 		_force2D(Parameters::defaultOdomForce2D()),
+		_fillInfoData(Parameters::defaultOdomFillInfoData()),
 		_resetCurrentCount(0)
 {
-	Parameters::parse(parameters, Parameters::kOdomLinearUpdate(), _linearUpdate);
-	Parameters::parse(parameters, Parameters::kOdomAngularUpdate(), _angularUpdate);
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
 	Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
 	Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
 	Parameters::parse(parameters, Parameters::kOdomRefineIterations(), _refineIterations);
-	Parameters::parse(parameters, Parameters::kOdomFeaturesRatio(), _featuresRatio);
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
 	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
 	Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
+	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
 }
 
 void Odometry::reset(const Transform & initialPose)
@@ -105,27 +101,14 @@ void Odometry::reset(const Transform & initialPose)
 	}
 }
 
-bool Odometry::isLargeEnoughTransform(const Transform & transform)
-{
-	float x,y,z, roll,pitch,yaw;
-	transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-	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(const SensorData & data, OdometryInfo * info)
 {
-	UTimer time;
 	if(_pose.isNull())
 	{
 		_pose.setIdentity(); // initialized
 	}
 
+	UTimer time;
 	Transform t = this->computeTransform(data, info);
 
 	if(info)
@@ -145,7 +128,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 			t = Transform(x,y,0, 0,0,yaw);
 		}
 
-		return _pose *= t;
+		return _pose *= t; // updated
 	}
 	else if(_resetCurrentCount > 0)
 	{
@@ -154,10 +137,11 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		--_resetCurrentCount;
 		if(_resetCurrentCount == 0)
 		{
-			UWARN("Odometry automatically reset!");
+			UWARN("Odometry automatically reset to latest pose!");
-			this->reset();
+			this->reset(_pose);
 		}
 	}
+
 	return Transform();
 }
 
@@ -242,7 +226,12 @@ Transform OdometryBOW::computeTransform(
 	UTimer timer;
 	Transform output;
 
-	double variance = -1;
+	if(info)
+	{
+		info->type = 0;
+	}
+
+	double variance = 0;
 	int inliers = 0;
 	int correspondences = 0;
 	int nFeatures = 0;
@@ -254,18 +243,17 @@ Transform OdometryBOW::computeTransform(
 		if(newSignature)
 		{
 			nFeatures = (int)newSignature->getWords().size();
+			if(this->isInfoDataFilled() && info)
+			{
+				info->words = newSignature->getWords();
+			}
 		}
 
 		if(previousSignature && newSignature)
 		{
 			Transform transform;
 			std::set<int> uniqueCorrespondences;
-			if(newSignature->getWords3().size() < (unsigned int)(this->getFeaturesRatio() * float(previousSignature->getWords3().size())))
+			if(!localMap_.empty() && !newSignature->getWords3().empty())
-			{
-				UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-						this->getFeaturesRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
-			}
-			else if(!localMap_.empty() && !newSignature->getWords3().empty())
 			{
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
 				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
@@ -283,6 +271,11 @@ Transform OdometryBOW::computeTransform(
 
 				UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
 
+				if(this->isInfoDataFilled() && info)
+				{
+					info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end());
+				}
+
 				correspondences = (int)inliers1->size();
 				if((int)inliers1->size() >= this->getMinInliers())
 				{
@@ -307,6 +300,15 @@ Transform OdometryBOW::computeTransform(
 						transform = this->getPose().inverse() * transform;
 
 						UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
+
+						if(this->isInfoDataFilled() && info && inliersV.size())
+						{
+							info->wordInliers.resize(inliersV.size());
+							for(unsigned int i=0; i<inliersV.size(); ++i)
+							{
+								info->wordInliers[i] = info->wordMatches[inliersV[i]];
+							}
+						}
 					}
 					else
 					{
@@ -388,54 +390,46 @@ Transform OdometryBOW::computeTransform(
 			else
 			{
 				output = transform;
-				if(!isLargeEnoughTransform(transform))
+				// remove words if history max size is reached
+				while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
 				{
-					// Transform not large enough, keep the old signature
+					int nodeId = *_memory->getStMem().begin();
-					_memory->deleteLocation(newSignature->id());
+					std::list<int> removedPts;
+					_memory->deleteLocation(nodeId, &removedPts);
+					for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
+					{
+						localMap_.erase(*iter);
+					}
 				}
-				else
+
+				if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
 				{
-					// remove words if history max size is reached
+					UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
-					while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
+							(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
-					{
+				}
-						int nodeId = *_memory->getStMem().begin();
-						std::list<int> removedPts;
-						_memory->deleteLocation(nodeId, &removedPts);
-						for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
-						{
-							localMap_.erase(*iter);
-						}
-					}
 
-					if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
+				// update local map
+				std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
+				Transform t = this->getPose()*output;
+				for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
+				{
+					// Only add unique words not in local map
+					if(newSignature->getWords3().count(*iter) == 1)
 					{
-						UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
+						// keep old word
-								(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
+						if(localMap_.find(*iter) == localMap_.end())
-					}
-
-					// update local map
-					std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
-					Transform t = this->getPose()*output;
-					for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
-					{
-						// Only add unique words not in local map
-						if(newSignature->getWords3().count(*iter) == 1)
 						{
-							// keep old word
+							const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
-							if(localMap_.find(*iter) == localMap_.end())
+							if(pcl::isFinite(pt))
 							{
-								const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+								pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
-								if(pcl::isFinite(pt))
+								localMap_.insert(std::make_pair(*iter, pt2));
-								{
-									pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
-									localMap_.insert(std::make_pair(*iter, pt2));
-								}
 							}
 						}
-						else
+					}
-						{
+					else
-							localMap_.erase(*iter);
+					{
-						}
+						localMap_.erase(*iter);
 					}
 				}
 			}
@@ -443,28 +437,37 @@ Transform OdometryBOW::computeTransform(
 		else if(!previousSignature && newSignature)
 		{
 			localMap_.clear();
-			output.setIdentity();
 
 			int count = 0;
 			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
-			Transform t = this->getPose(); // initial pose maybe not identity...
+			if((int)uniques.size() >= this->getMinInliers())
-			for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
 			{
-				// Only add unique words
+				output.setIdentity();
-				if(newSignature->getWords3().count(*iter) == 1)
+
+				Transform t = this->getPose(); // initial pose maybe not identity...
+				for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
 				{
-					const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
+					// Only add unique words
-					if(pcl::isFinite(pt))
+					if(newSignature->getWords3().count(*iter) == 1)
 					{
-						pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
+						const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
-						localMap_.insert(std::make_pair(*iter, pt2));
+						if(pcl::isFinite(pt))
-					}
+						{
-					else
+							pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
-					{
+							localMap_.insert(std::make_pair(*iter, pt2));
-						++count;
+						}
+						else
+						{
+							++count;
+						}
 					}
 				}
 			}
+			else
+			{
+				// not enough features, just delete it
+				_memory->deleteLocation(newSignature->id());
+			}
 			UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
 		}
 
@@ -480,12 +483,13 @@ Transform OdometryBOW::computeTransform(
 		info->localMapSize = (int)localMap_.size();
 	}
 
-	UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d local_map=%d[%d] dict=%d nodes=%d",
+	UINFO("Odom update time = %fs out=[%s] features=%d inliers=%d/%d variance=%f local_map=%d[%d] dict=%d nodes=%d",
 			timer.elapsed(),
 			output.prettyPrint().c_str(),
 			nFeatures,
 			inliers,
 			correspondences,
+			variance,
 			(int)uUniqueKeys(localMap_).size(),
 			(int)localMap_.size(),
 			(int)_memory->getVWDictionary()->getVisualWords().size(),
@@ -553,6 +557,11 @@ Transform OdometryOpticalFlow::computeTransform(
 {
 	UDEBUG("");
 
+	if(info)
+	{
+		info->type = 1;
+	}
+
 	if(!data.rightImage().empty())
 	{
 		//stereo
@@ -572,7 +581,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 	UTimer timer;
 	Transform output;
 
-	double variance = -1;
+	double variance = 0;
 	int inliers = 0;
 	int correspondences = 0;
 
@@ -642,21 +651,6 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
 
 			UDEBUG("");
-			std::vector<cv::KeyPoint> lastKpts, newKpts;
-			/*cv::KeyPoint::convert(lastCornersKept, lastKpts);
-			cv::KeyPoint::convert(newCornersKept, newKpts);
-			std::vector<cv::DMatch> good_matches(lastKpts.size());
-			for(unsigned int i=0; i<good_matches.size(); ++i)
-			{
-				good_matches[i].trainIdx = i;
-				good_matches[i].queryIdx = i;
-			}
-
-			cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
-						   good_matches, imgMatches_, cv::Scalar::all(-1), cv::Scalar::all(-1),
-						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
-			UDEBUG("");*/
-
 			std::vector<unsigned char> statusNew;
 			std::vector<float> errNew;
 			std::vector<cv::Point2f> newCornersKeptRight;
@@ -678,8 +672,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			correspondencesLast->resize(statusLast.size());
 			correspondencesNew->resize(statusLast.size());
 			int oi = 0;
-			lastKpts.resize(statusLast.size());
+			if(this->isInfoDataFilled() && info)
-			newKpts.resize(statusLast.size());
+			{
+				info->refCorners.resize(statusLast.size());
+				info->newCorners.resize(statusLast.size());
+			}
 			for(unsigned int i=0; i<statusLast.size(); ++i)
 			{
 				if(statusLast[i] && statusNew[i])
@@ -708,8 +705,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 							newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
 							correspondencesLast->at(oi) = lastPt3D;
 							correspondencesNew->at(oi) = newPt3D;
-							lastKpts[oi].pt = lastCornersKept[i];
+							if(this->isInfoDataFilled() && info)
-							newKpts[oi].pt = newCornersKept[i];
+							{
+								info->refCorners[oi].pt = lastCornersKept[i];
+								info->newCorners[oi].pt = newCornersKept[i];
+							}
 							++oi;
 						}
 					}
@@ -717,26 +717,15 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			}// end loop
 			correspondencesLast->resize(oi);
 			correspondencesNew->resize(oi);
-			lastKpts.resize(oi);
+			if(this->isInfoDataFilled() && info)
-			newKpts.resize(oi);
+			{
+				info->refCorners.resize(oi);
+				info->newCorners.resize(oi);
+			}
 			correspondences = oi;
 			refCorners3D_ = correspondencesNew;
 			UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
 
-			/*good_matches.resize(lastKpts.size());
-			for(unsigned int i=0; i<good_matches.size(); ++i)
-			{
-				good_matches[i].trainIdx = i;
-				good_matches[i].queryIdx = i;
-			}
-
-			cv::Mat imgInliers;
-			cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
-						   good_matches, imgInliers, cv::Scalar::all(-1), cv::Scalar::all(-1),
-						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
-			imgMatches_.push_back(imgInliers);
-			UDEBUG("");*/
-
 			if(correspondences >= this->getMinInliers())
 			{
 				std::vector<int> inliersV;
@@ -757,15 +746,10 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 					output.setNull();
 					UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
 				}
-
+				else if(this->isInfoDataFilled() && info && !output.isNull())
-				//if(correspondencesLast->size() >= 6)
+				{
-				//{
+					info->cornerInliers = inliersV;
-				//	UWARN("saved pcd");
+				}
-				//	pcl::io::savePCDFile("last.pcd", *correspondencesLast);
-				//	pcl::io::savePCDFile("new.pcd", *correspondencesNew);
-					//correspondencesNew = util3d::transformPointCloud(correspondencesNew, output);
-					//pcl::io::savePCDFile("new2.pcd", *correspondencesNew);
-				//}
 			}
 			else
 			{
@@ -815,12 +799,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			}
 		}
 
-		if(refCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(refCorners_.size())))
+		if((int)newCorners.size() > this->getMinInliers())
-		{
-			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-					this->getFeaturesRatio()*100.0f, newCorners.size(), refCorners_.size());
-		}
-		else if((int)newCorners.size() > this->getMinInliers())
 		{
 			refFrame_ = newLeftFrame;
 			refRightFrame_ = newRightFrame;
@@ -830,12 +809,9 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 		{
 			UWARN("Too low 2D corners (%d), ignoring new frame...",
 					(int)newCorners.size());
+			output.setNull();
 		}
 	}
-	else if(!output.isNull())
-	{
-		output.setNull();
-	}
 
 	if(info)
 	{
@@ -862,7 +838,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 	UTimer timer;
 	Transform output;
 
-	double variance = -1;
+	double variance = 0;
 	int inliers = 0;
 	int correspondences = 0;
 
@@ -877,9 +853,6 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 		newFrame = data.image().clone();
 	}
 
-	float updatePixels = 0.0f;
-	bool updateFrame = false;
-
 	std::vector<cv::Point2f> newCorners;
 	if(!refFrame_.empty() && refCorners_.size() && refCorners3D_->size())
 	{
@@ -904,8 +877,11 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 		correspondencesNew->resize(refCorners_.size());
 		int oi=0;
 
-		std::vector<cv::KeyPoint> lastKpts(refCorners_.size());
+		if(this->isInfoDataFilled() && info)
-		std::vector<cv::KeyPoint> newKpts(refCorners_.size());
+		{
+			info->refCorners.resize(refCorners_.size());
+			info->newCorners.resize(refCorners_.size());
+		}
 
 		UASSERT(refCorners_.size() == refCorners3D_->size());
 		UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
@@ -932,8 +908,11 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 					cv::Point2f diff = newCorners[i]-refCorners_[i];
 					sumSqrdDistance += diff.x*diff.x + diff.y*diff.y;
 
-					lastKpts[oi].pt = refCorners_[i];
+					if(this->isInfoDataFilled() && info)
-					newKpts[oi].pt = newCorners[i];
+					{
+						info->refCorners[oi].pt = refCorners_[i];
+						info->newCorners[oi].pt = newCorners[i];
+					}
 
 					++oi;
 				}
@@ -945,19 +924,12 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 			}
 		}
 		UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
-		float meanPixel = -1;
-		if(oi)
-		{
-			float meanPixel = sumSqrdDistance/(float)oi;
-			if(meanPixel >= updatePixels*updatePixels)
-			{
-				updateFrame = true;
-			}
-		}
-		UDEBUG("mean pixel distance = %f", meanPixel);
 
-		lastKpts.resize(oi);
+		if(this->isInfoDataFilled() && info)
-		newKpts.resize(oi);
+		{
+			info->refCorners.resize(oi);
+			info->newCorners.resize(oi);
+		}
 		correspondencesLast->resize(oi);
 		correspondencesNew->resize(oi);
 		correspondences = oi;
@@ -981,6 +953,10 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 				output.setNull();
 				UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
 			}
+			else if(this->isInfoDataFilled() && info && !output.isNull())
+			{
+				info->cornerInliers = inliersV;
+			}
 
 			/*std::vector<cv::DMatch> good_matches(lastKpts.size());
 			for(unsigned int i=0; i<good_matches.size(); ++i)
@@ -1002,11 +978,10 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 	{
 		//return Identity
 		output = Transform::getIdentity();
-		updateFrame = true;
 	}
 
 	newCorners.clear();
-	if(!output.isNull() && updateFrame)
+	if(!output.isNull())
 	{
 		// Copy or generate new keypoints
 		if(data.keypoints().size())
@@ -1040,12 +1015,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 			}
 		}
 
-		if(refCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(refCorners_.size())))
+		if((int)newCorners.size() > this->getMinInliers())
-		{
-			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-					this->getFeaturesRatio()*100.0f, newCorners.size(), refCorners_.size());
-		}
-		else if((int)newCorners.size() > this->getMinInliers())
 		{
 			// get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow)
 			pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
@@ -1084,18 +1054,16 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 			{
 				UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
 						(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
+				output.setNull();
 			}
 		}
 		else
 		{
 			UWARN("Too low 2D corners (%d), ignoring new frame...",
 					(int)newCorners.size());
+			output.setNull();
 		}
 	}
-	else if(!output.isNull())
-	{
-		output = Transform::getIdentity();
-	}
 
 	if(info)
 	{
@@ -1105,12 +1073,12 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 		info->matches = correspondences;
 	}
 
-	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
+	UINFO("Odom update time = %fs inliers=%d/%d, variance=%f, new corners=%d",
 			timer.elapsed(),
 			inliers,
 			correspondences,
-			(int)newCorners.size(),
+			variance,
-			updateFrame||output.isNull()?"true":"false");
+			(int)newCorners.size());
 	return output;
 }
 
@@ -1150,7 +1118,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo *
 	Transform output;
 
 	bool hasConverged = false;
-	double variance = -1;
+	double variance = 0;
 	unsigned int minPoints = 100;
 	if(!data.depth().empty())
 	{

corelib/src/Rtabmap.cpp

@@ -771,30 +771,20 @@ bool Rtabmap::process(const SensorData & data)
 			if(_memory->getLastWorkingSignature())
 			{
 				const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
-				if(!lastPose.isIdentity() && data.pose().isIdentity())
+				Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
+				float x,y,z, roll,pitch,yaw;
+				lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+				if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
 				{
 					int mapId = _memory->incrementMapId();
-					UWARN("Odometry is reset (transform identity detected). A new map (%d) is created!", mapId);
+					UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
+							_newMapOdomChangeDistance,
+							mapId,
+							lastPose.prettyPrint().c_str(),
+							data.pose().prettyPrint().c_str());
 					_optimizedPoses.clear();
 					_constraints.clear();
 				}
-				else
-				{
-					Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
-					float x,y,z, roll,pitch,yaw;
-					lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-					if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
-					{
-						int mapId = _memory->incrementMapId();
-						UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
-								_newMapOdomChangeDistance,
-								mapId,
-								lastPose.prettyPrint().c_str(),
-								data.pose().prettyPrint().c_str());
-						_optimizedPoses.clear();
-						_constraints.clear();
-					}
-				}
 			}
 		}
 	}

corelib/src/RtabmapThread.cpp

@@ -46,7 +46,9 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_rate(Parameters::defaultRtabmapDetectionRate()),
 		_frameRateTimer(new UTimer()),
 		_rtabmap(rtabmap),
-		_paused(false)
+		_paused(false),
+		lastPose_(Transform::getIdentity()),
+		_variance(0)
 
 {
 	UASSERT(rtabmap != 0);
@@ -82,6 +84,8 @@ void RtabmapThread::clearBufferedData()
 	_dataMutex.lock();
 	{
 		_dataBuffer.clear();
+		lastPose_.setIdentity();
+		_variance = 0;
 	}
 	_dataMutex.unlock();
 }
@@ -253,6 +257,10 @@ void RtabmapThread::handleEvent(UEvent* event)
 		{
 			this->addData(e->data());
 		}
+		else
+		{
+			lastPose_.setNull();
+		}
 	}
 	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
 	{
@@ -392,7 +400,7 @@ void RtabmapThread::process()
 {
 	SensorData data;
 	getData(data);
-	if(data.isValid())
+	if(data.isValid() && _state.empty())
 	{
 		if(_rtabmap->getMemory())
 		{
@@ -421,6 +429,19 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			return;
 		}
 
+		if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
+		{
+			UWARN("Odometry is reset (identity pose detected). Increment map id!");
+			pushNewState(kStateTriggeringMap);
+			_variance = 0;
+		}
+
+		lastPose_ = sensorData.pose();
+		if(sensorData.poseVariance() > _variance)
+		{
+			_variance = sensorData.poseVariance();
+		}
+
 		if(_rate>0.0f)
 		{
 			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
@@ -434,6 +455,12 @@ void RtabmapThread::addData(const SensorData & sensorData)
 		_dataMutex.lock();
 		{
 			_dataBuffer.push_back(sensorData);
+			if(_variance <= 0)
+			{
+				_variance = 1.0f;
+			}
+			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _variance);
+			_variance = 0;
 			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
 			{
 				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");

corelib/src/util3d.cpp

@@ -1504,7 +1504,7 @@ Transform transformFromXYZCorrespondences(
 
 	if(varianceOut)
 	{
-		*varianceOut = 1.0f;
+		*varianceOut = 1.0;
 	}
 	Transform transform;
 	if(cloud1->size() >=3 && cloud1->size() == cloud2->size())

guilib/include/rtabmap/gui/ImageView.h

@@ -63,7 +63,8 @@ public:
 	void setImageDepthShown(bool shown);
 	void setLinesShown(bool shown);
 
-	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords);
+	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color = QColor(255, 255, 0, 70));
+	void setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color = QColor(255, 255, 0, 70));
 	void setImage(const QImage & image);
 	void setImageDepth(const QImage & image);
 	void setFeatureColor(int id, const QColor & color);

guilib/include/rtabmap/gui/KeypointItem.h

@@ -43,6 +43,8 @@ public:
 	KeypointItem(qreal x, qreal y, int r, const QString & info, const QColor & color = Qt::green, QGraphicsItem * parent = 0);
 	virtual ~KeypointItem();
 
+	void setColor(const QColor & color);
+
 protected:
 	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event );
 	virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event );

guilib/src/ImageView.cpp

@@ -201,6 +201,14 @@ void ImageView::updateOpacity()
 			_imageDepth->setGraphicsEffect(0);
 		}
 	}
+	else if(_image)
+	{
+		_image->setGraphicsEffect(0);
+	}
+	else if(_imageDepth)
+	{
+		_imageDepth->setGraphicsEffect(0);
+	}
 }
 
 void ImageView::updateZoom()
@@ -242,13 +250,12 @@ void ImageView::wheelEvent(QWheelEvent * e)
 	this->setMatrix(matrix);
 }
 
-void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
+void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const QColor & color)
 {
 	qDeleteAll(_features);
 	_features.clear();
 
 	rtabmap::KeypointItem * item = 0;
-	int alpha = 70;
 	for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
 	{
 		const cv::KeyPoint & r = (*i).second;
@@ -260,9 +267,35 @@ void ImageView::setFeatures(const std::multimap<int, cv::KeyPoint> & refWords)
 								"X = %5\n"
 								"Y = %6\n"
 								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
-		float radius = r.size*1.2/9.*2;
+		float radius = r.size/2.0f;
+		item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, color);
 
-		item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, QColor(255, 255, 0, alpha));
+		scene()->addItem(item);
+		_features.insert(id, item);
+		item->setVisible(_showFeatures->isChecked());
+		item->setZValue(1);
+	}
+}
+
+void ImageView::setFeatures(const std::vector<cv::KeyPoint> & features, const QColor & color)
+{
+	qDeleteAll(_features);
+	_features.clear();
+
+	rtabmap::KeypointItem * item = 0;
+	for(unsigned int i = 0; i< features.size(); ++i )
+	{
+		const cv::KeyPoint & r = features[i];
+		int id = i;
+		QString info = QString( "WordRef = %1\n"
+								"Laplacian = %2\n"
+								"Dir = %3\n"
+								"Hessian = %4\n"
+								"X = %5\n"
+								"Y = %6\n"
+								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
+		float radius = (r.size==0?3:r.size)/2.0f;
+		item = new rtabmap::KeypointItem(r.pt.x-radius, r.pt.y-radius, radius*2, info, color);
 
 		scene()->addItem(item);
 		_features.insert(id, item);
@@ -308,8 +341,7 @@ void ImageView::setFeatureColor(int id, const QColor & color)
 	{
 		for(int i=0; i<items.size(); ++i)
 		{
-			items[i]->setPen(QPen(color));
+			items[i]->setColor(color);
-			items[i]->setBrush(QBrush(color));
 		}
 	}
 	else

guilib/src/KeypointItem.cpp

@@ -39,8 +39,7 @@ KeypointItem::KeypointItem(qreal x, qreal y, int r, const QString & info, const
 	_info(info),
 	_placeHolder(0)
 {
-	this->setPen(QPen(color));
+	this->setColor(color);
-	this->setBrush(QBrush(color));
 	this->setAcceptsHoverEvents(true);
 	this->setFlag(QGraphicsItem::ItemIsFocusable, true);
 	_width = pen().width();
@@ -48,10 +47,16 @@ KeypointItem::KeypointItem(qreal x, qreal y, int r, const QString & info, const
 
 KeypointItem::~KeypointItem()
 {
-	/*if(_placeHolder)
+	if(_placeHolder)
 	{
 		delete _placeHolder;
-	}*/
+	}
+}
+
+void KeypointItem::setColor(const QColor & color)
+{
+	this->setPen(QPen(color));
+	this->setBrush(QBrush(color));
 }
 
 void KeypointItem::showDescription()

guilib/src/MainWindow.cpp

@@ -566,9 +566,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	else if(anEvent->getClassName().compare("OdometryEvent") == 0)
 	{
 		OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
-		if((_ui->dockWidget_cloudViewer->isVisible() || _ui->dockWidget_odometry->isVisible()) &&
+		if(_lastOdometryProcessed && !_processingStatistics)
-		   _lastOdometryProcessed &&
-		   !_processingStatistics)
 		{
 			_lastOdometryProcessed = false; // if we receive too many odometry events!
 			emit odometryReceived(odomEvent->data(), odomEvent->info());
@@ -596,6 +594,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 
 void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
 {
+	UTimer time;
 	Transform pose = data.pose();
 	bool lost = false;
 	_ui->imageView_odometry->resetTransform();
@@ -725,6 +724,16 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 	if(_ui->dockWidget_odometry->isVisible() &&
 	   !data.image().empty())
 	{
+		int alpha = _preferencesDialog->getKeypointsOpacity()*255/100;
+		if(info.type == 0)
+		{
+			_ui->imageView_odometry->setFeatures(info.words, QColor(255,255,0, alpha));
+		}
+		else if(info.type == 1)
+		{
+			_ui->imageView_odometry->setFeatures(info.refCorners, QColor(255,0,0, alpha));
+		}
+		_ui->imageView_odometry->clearLines();
 		if(lost)
 		{
 			_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
@@ -736,6 +745,35 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 			_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
 			_ui->imageView_odometry->setImageShown(true);
 			_ui->imageView_odometry->setImageDepthShown(false);
+
+			if(info.type == 0)
+			{
+				for(unsigned int i=0; i<info.wordMatches.size(); ++i)
+				{
+					_ui->imageView_odometry->setFeatureColor(info.wordMatches[i], QColor(255,0,0, alpha)); // outliers
+				}
+				for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+				{
+					_ui->imageView_odometry->setFeatureColor(info.wordInliers[i], QColor(0,255,0, alpha)); // inliers
+				}
+			}
+			else if(info.type == 1)
+			{
+				//draw lines
+				UASSERT(info.refCorners.size() == info.newCorners.size());
+				for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
+				{
+					_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], QColor(0,255,0, alpha)); // inliers
+					QGraphicsLineItem * item = _ui->imageView_odometry->scene()->addLine(
+							info.refCorners[info.cornerInliers[i]].pt.x,
+							info.refCorners[info.cornerInliers[i]].pt.y,
+							info.newCorners[info.cornerInliers[i]].pt.x,
+							info.newCorners[info.cornerInliers[i]].pt.y,
+							QPen(QColor(0, 0, 255, alpha)));
+					item->setVisible(_ui->imageView_odometry->isLinesShown());
+					item->setZValue(1);
+				}
+			}
 		}
 
 		_ui->imageView_odometry->resetZoom();
@@ -753,6 +791,8 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
 	{
 		this->captureScreen();
 	}
+
+	_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
 }
 
 void MainWindow::processStats(const rtabmap::Statistics & stat)
@@ -1815,7 +1855,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 								"X = %5\n"
 								"Y = %6\n"
 								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
-		float radius = r.size*1.2/9.*2;
+		float radius = r.size/2.0f;
 		if(uContains(loopWords, id))
 		{
 			// PINK = FOUND IN LOOP SIGNATURE
@@ -1864,7 +1904,7 @@ void MainWindow::drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords
 								"X = %5\n"
 								"Y = %6\n"
 								"Size = %7").arg(id).arg(1).arg(r.angle).arg(r.response).arg(r.pt.x).arg(r.pt.y).arg(r.size);
-		float radius = r.size*1.2/9.*2;
+		float radius = r.size/2.0f;
 		if(uContains(refWords, id))
 		{
 			// PINK = FOUND IN LOOP SIGNATURE
@@ -1945,7 +1985,7 @@ void MainWindow::resizeEvent(QResizeEvent* anEvent)
 {
 	_ui->imageView_source->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
 	_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
-	_ui->imageView_source->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
+	_ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
 	_ui->imageView_source->resetZoom();
 	_ui->imageView_loopClosure->resetZoom();
 	_ui->imageView_odometry->resetZoom();

guilib/src/PreferencesDialog.cpp

@@ -482,17 +482,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
 	_ui->odom_type->setObjectName(Parameters::kOdomFeatureType().c_str());
-	_ui->odom_linearUpdate->setObjectName(Parameters::kOdomLinearUpdate().c_str());
-	_ui->odom_angularUpdate->setObjectName(Parameters::kOdomAngularUpdate().c_str());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
 	_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxFeatures().c_str());
-	_ui->odom_ratio->setObjectName(Parameters::kOdomFeaturesRatio().c_str());
 	_ui->odom_inlierDistance->setObjectName(Parameters::kOdomInlierDistance().c_str());
 	_ui->odom_iterations->setObjectName(Parameters::kOdomIterations().c_str());
 	_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
 	_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
 	_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
 	_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
+	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
 	_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
 
 	//Odometry BOW

guilib/src/ui/DatabaseViewer.ui

@@ -792,6 +792,9 @@
          <property name="text">
           <string/>
          </property>
+         <property name="checked">
+          <bool>true</bool>
+         </property>
         </widget>
        </item>
        <item row="2" column="0">

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-305</y>
+            <y>0</y>
             <width>744</width>
-            <height>1252</height>
+            <height>1056</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>23</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6261,7 +6261,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_27" columnstretch="0,0">
-                      <item row="4" column="1">
+                      <item row="2" column="1">
                        <widget class="QLabel" name="label_47">
                         <property name="text">
                          <string>Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).</string>
@@ -6288,55 +6288,13 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </item>
                        </widget>
                       </item>
-                      <item row="3" column="1">
+                      <item row="2" column="0">
-                       <widget class="QLabel" name="label_46">
-                        <property name="text">
-                         <string>Angular update: minimum angular distance to update the odometry.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="3" column="0">
-                       <widget class="QDoubleSpinBox" name="odom_angularUpdate">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.015000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="4" column="0">
                        <widget class="QSpinBox" name="odom_countdown">
                         <property name="maximum">
                          <number>999999</number>
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="0">
-                       <widget class="QDoubleSpinBox" name="odom_linearUpdate">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.015000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="1" column="0">
                        <widget class="QComboBox" name="odom_type">
                         <property name="sizeAdjustPolicy">
@@ -6384,16 +6342,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </item>
                        </widget>
                       </item>
-                      <item row="2" column="1">
-                       <widget class="QLabel" name="label_45">
-                        <property name="text">
-                         <string>Linear update: minimum linear distance to update the odometry.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="1" column="1">
                        <widget class="QLabel" name="label_44">
                         <property name="text">
@@ -6416,14 +6364,14 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="0">
+                      <item row="3" column="0">
                        <widget class="QCheckBox" name="odom_force2D">
                         <property name="text">
                          <string/>
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
+                      <item row="3" column="1">
                        <widget class="QLabel" name="label_196">
                         <property name="text">
                          <string>Force 2D transform (3DoF: x,y and yaw).</string>
@@ -6433,6 +6381,23 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_221">
+                        <property name="text">
+                         <string>Fill info with data (inliers/outliers features to be shown in Odometry view).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="4" column="0">
+                       <widget class="QCheckBox" name="odom_fillInfoData">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -6545,38 +6510,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
-                       <item row="4" column="0">
-                        <widget class="QDoubleSpinBox" name="odom_ratio">
-                         <property name="suffix">
-                          <string/>
-                         </property>
-                         <property name="decimals">
-                          <number>2</number>
-                         </property>
-                         <property name="minimum">
-                          <double>0.000000000000000</double>
-                         </property>
-                         <property name="maximum">
-                          <double>1.000000000000000</double>
-                         </property>
-                         <property name="singleStep">
-                          <double>0.010000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>0.500000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="4" column="1">
-                        <widget class="QLabel" name="label_90">
-                         <property name="text">
-                          <string>Minimum ratio of keypoints between the current image and the last image to compute odometry.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                       </layout>
                      </widget>
                     </item>

tools/OdometryViewer/main.cpp

@@ -60,9 +60,6 @@ void showUsage()
 			"  -min #                    Minimum inliers to accept the transform (default 20)\n"
 			"  -depth #.#                Maximum features depth (default 5.0 m)\n"
 			"  -i #                      RANSAC/ICP iterations (default 30)\n"
-			"  -r #.#                    Words ratio (default 0.5)\n"
-			"  -lu #                     Linear update (default 0.0 m)\n"
-			"  -au #                     Angular update (default 0.0 radian)\n"
 			"  -reset #                  Reset countdown (default 0 = disabled)\n"
 			"  -gpu                      Use GPU\n"
 			"  -lh #                     Local history (default 1000)\n"
@@ -105,11 +102,8 @@ int main (int argc, char * argv[])
 	float distance = 0.01;
 	int maxWords = 0;
 	int minInliers = 20;
-	float wordsRatio = 0.5;
 	float maxDepth = 5.0f;
 	int iterations = 30;
-	float linearUpdate = 0.0f;
-	float angularUpdate = 0.0f;
 	int resetCountdown = 0;
 	int decimation = 4;
 	float voxel = 0.005;
@@ -347,57 +341,6 @@ int main (int argc, char * argv[])
 			}
 			continue;
 		}
-		if(strcmp(argv[i], "-r") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				wordsRatio = std::atof(argv[i]);
-				if(wordsRatio < 0.0f || wordsRatio > 1.0f)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-lu") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				linearUpdate = std::atof(argv[i]);
-				if(linearUpdate < 0.0f)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
-		if(strcmp(argv[i], "-au") == 0)
-		{
-			++i;
-			if(i < argc)
-			{
-				angularUpdate = std::atof(argv[i]);
-				if(angularUpdate < 0.0f)
-				{
-					showUsage();
-				}
-			}
-			else
-			{
-				showUsage();
-			}
-			continue;
-		}
 		if(strcmp(argv[i], "-reset") == 0)
 		{
 			++i;
@@ -655,8 +598,6 @@ int main (int argc, char * argv[])
 	UINFO("Maximum clouds shown =    %d", maxClouds);
 	UINFO("Delay =                   %f s", sec);
 	UINFO("Max depth =               %f", maxDepth);
-	UINFO("Linear update =           %f", linearUpdate);
-	UINFO("Angular update =          %f", angularUpdate);
 	UINFO("Reset odometry coutdown = %d", resetCountdown);
 	UINFO("Local history =           %d", localHistory);
 
@@ -667,8 +608,6 @@ int main (int argc, char * argv[])
 	rtabmap::ParametersMap parameters;
 
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomLinearUpdate(), uNumber2Str(linearUpdate)));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomAngularUpdate(), uNumber2Str(angularUpdate)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
 
@@ -693,7 +632,6 @@ int main (int argc, char * argv[])
 			UINFO("Nearest neighbor ratio =  %f", nndr);
 			UINFO("Max features =            %d", maxWords);
 			UINFO("Min inliers =             %d", minInliers);
-			UINFO("Words ratio =             %f", wordsRatio);
 			UINFO("Inlier maximum correspondences distance = %f", distance);
 			UINFO("RANSAC iterations =       %d", iterations);
 			UINFO("GPU =                     %s", gpu?"true":"false");
@@ -702,7 +640,6 @@ int main (int argc, char * argv[])
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeaturesRatio(), uNumber2Str(wordsRatio)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
 			if(odomType == 0)