Added PnP Pose estimation based Odometry option. Added sensor icons in menu.

corelib/include/rtabmap/core/Odometry.h

@@ -70,6 +70,9 @@ public:
 	int getRefineIterations() const {return _refineIterations;}
 	float getMaxDepth() const {return _maxDepth;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
+	bool isPnPEstimationUsed() const {return _pnpEstimation;}
+	double  getPnPReprojError() const {return _pnpReprojError;}
+	int  getPnPFlags() const {return _pnpFlags;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
@@ -85,6 +88,9 @@ private:
 	int _resetCountdown;
 	bool _force2D;
 	bool _fillInfoData;
+	bool _pnpEstimation;
+	double _pnpReprojError;
+	int _pnpFlags;
 	Transform _pose;
 	int _resetCurrentCount;
 

corelib/include/rtabmap/core/Parameters.h

@@ -318,6 +318,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
 	RTABMAP_PARAM(Odom, Force2D, 		        bool, false,     "Force 2D transform (3Dof: x,y and yaw).");
 	RTABMAP_PARAM(Odom, FillInfoData, 		    bool, true,     "Fill info with data (inliers/outliers features).");
+	RTABMAP_PARAM(Odom, PnPEstimation, 		    bool, false,     "(PnP) Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences.");
+	RTABMAP_PARAM(Odom, PnPReprojError, 		double, 8.0,     "PnP reprojection error.");
+	RTABMAP_PARAM(Odom, PnPFlags, 				int, 0,    	      "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 
 	// 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/src/Odometry.cpp

@@ -67,6 +67,9 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
 		_force2D(Parameters::defaultOdomForce2D()),
 		_fillInfoData(Parameters::defaultOdomFillInfoData()),
+		_pnpEstimation(Parameters::defaultOdomPnPEstimation()),
+		_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
+		_pnpFlags(Parameters::defaultOdomPnPFlags()),
 		_resetCurrentCount(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
@@ -79,6 +82,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
 	Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
+	Parameters::parse(parameters, Parameters::kOdomPnPEstimation(), _pnpEstimation);
+	Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
+	Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
+	UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
 }
 
 void Odometry::reset(const Transform & initialPose)
@@ -252,8 +259,109 @@ Transform OdometryBOW::computeTransform(
 		if(previousSignature && newSignature)
 		{
 			Transform transform;
-			std::set<int> uniqueCorrespondences;
+			if((int)localMap_.size() >= this->getMinInliers())
-			if(!localMap_.empty() && !newSignature->getWords3().empty())
+			{
+				if(this->isPnPEstimationUsed())
+				{
+					if((int)newSignature->getWords().size() >= this->getMinInliers())
+					{
+						// find correspondences
+						std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
+						std::vector<cv::Point3f> objectPoints(ids.size());
+						std::vector<cv::Point2f> imagePoints(ids.size());
+						int oi=0;
+						std::vector<int> matches(ids.size());
+						for(unsigned int i=0; i<ids.size(); ++i)
+						{
+							if(localMap_.count(ids[i]) == 1)
+							{
+								pcl::PointXYZ pt = localMap_.find(ids[i])->second;
+								objectPoints[oi].x = pt.x;
+								objectPoints[oi].y = pt.y;
+								objectPoints[oi].z = pt.z;
+								imagePoints[oi] = newSignature->getWords().find(ids[i])->second.pt;
+								matches[oi++] = ids[i];
+							}
+						}
+						objectPoints.resize(oi);
+						imagePoints.resize(oi);
+						matches.resize(oi);
+
+						if(this->isInfoDataFilled() && info)
+						{
+							info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
+						}
+						correspondences = (int)matches.size();
+
+						if((int)matches.size() >= this->getMinInliers())
+						{
+							//PnPRansac
+							cv::Mat K = (cv::Mat_<double>(3,3) <<
+								data.fx(), 0, data.cx(),
+								0, data.fyOrBaseline(), data.cy(),
+								0, 0, 1);
+							Transform guess = (this->getPose() * data.localTransform()).inverse();
+							cv::Mat R = (cv::Mat_<double>(3,3) <<
+									(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
+									(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
+									(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
+							cv::Mat rvec(1,3, CV_64FC1);
+							cv::Rodrigues(R, rvec);
+							cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
+							std::vector<int> inliersV;
+							cv::solvePnPRansac(objectPoints,
+									imagePoints,
+									K,
+									cv::Mat(),
+									rvec,
+									tvec,
+									true,
+									this->getIterations(),
+									this->getPnPReprojError(),
+									this->getMinInliers(),
+									inliersV,
+									this->getPnPFlags());
+
+							inliers = (int)inliersV.size();
+							if((int)inliersV.size() >= this->getMinInliers())
+							{
+								cv::Rodrigues(rvec, R);
+								Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
+											   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
+											   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
+
+								// make it incremental
+								transform = (data.localTransform() * pnp * this->getPose()).inverse();
+
+								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] = matches[inliersV[i]];
+									}
+								}
+							}
+							else
+							{
+								UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
+							}
+						}
+						else
+						{
+							UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
+						}
+					}
+					else
+					{
+						UWARN("Not enough features in the new image (%d < %d)", (int)newSignature->getWords().size(), this->getMinInliers());
+					}
+				}
+				else
+				{
+					if((int)newSignature->getWords3().size() >= this->getMinInliers())
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
 						pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
@@ -261,6 +369,7 @@ Transform OdometryBOW::computeTransform(
 						// 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!
+						std::set<int> uniqueCorrespondences;
 						util3d::findCorrespondences(
 								localMap_,
 								newSignature->getWords3(),
@@ -279,9 +388,6 @@ Transform OdometryBOW::computeTransform(
 						correspondences = (int)inliers1->size();
 						if((int)inliers1->size() >= this->getMinInliers())
 						{
-					// transform new words in local map referential
-					//inliers2 = util3d::transformPointCloud<pcl::PointXYZ>(inliers2, this->getPose());
-
 							// the transform returned is global odometry pose, not incremental one
 							std::vector<int> inliersV;
 							transform = util3d::transformFromXYZCorrespondences(
@@ -313,64 +419,8 @@ Transform OdometryBOW::computeTransform(
 							else
 							{
 								UDEBUG("Odom transform null");
-						//pcl::io::savePCDFile("from.pcd", *inliers1);
-						//inliers2 = util3d::transformPointCloud(inliers2, this->getPose());
-						//pcl::io::savePCDFile("to.pcd", *inliers2);
-						//inliers2 = util3d::transformPointCloud(inliers2, transform);
-						//pcl::io::savePCDFile("to_t.pcd", *inliers2);
 							}
 
-					if(ULogger::level() == ULogger::kDebug)
-					{
-						float error3D = 0;
-					//	pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1Cloud(new pcl::PointCloud<pcl::PointXYZ>), inliers2Cloud(new pcl::PointCloud<pcl::PointXYZ>);
-						for(unsigned int i=0; i<inliersV.size(); ++i)
-						{
-							pcl::PointXYZ pt = util3d::transformPoint(inliers2->at(inliersV[i]), this->getPose()*transform);
-							error3D+=pcl::euclideanDistance(inliers1->at(inliersV[i]), pt);
-						//	inliers1Cloud->push_back(inliers1->at(inliersV[i]));
-						//	inliers2Cloud->push_back(pt);
-						}
-						error3D/=float(inliersV.size());
-						UDEBUG("3D error = %f", error3D);
-					}
-
-					/*if(inliersV.size() < 30 || fabs(transform.x()) > 0.3 || fabs(transform.y()) > 0.3 || fabs(transform.z()) > 0.3)
-					{
-						UWARN("Saved from.pcd, to.pcd and to_t.pcd");
-						pcl::io::savePCDFile("from.pcd", *inliers1);
-						pcl::io::savePCDFile("to.pcd", *inliers2);
-						inliers2 = util3d::transformPointCloud<pcl::PointXYZ>(inliers2, transform);
-						pcl::io::savePCDFile("to_t.pcd", *inliers2);
-
-						pcl::io::savePCDFile("inliersFrom.pcd", *inliers1Cloud);
-						pcl::io::savePCDFile("inliersTo.pcd", *inliers2Cloud);
-						inliers2Cloud = util3d::transformPointCloud<pcl::PointXYZ>(inliers2Cloud, transform);
-						pcl::io::savePCDFile("inliersTo_t.pcd", *inliers2Cloud);
-						exit(-1);
-					}*/
-					/*pcl::io::savePCDFile("from.pcd", *inliers1);
-					inliers2 = util3d::transformPointCloud(inliers2, this->getPose());
-					pcl::io::savePCDFile("to.pcd", *inliers2);
-					inliers2 = util3d::transformPointCloud(inliers2, transform);
-					pcl::io::savePCDFile("to_t.pcd", *inliers2);*/
-
-
-					/*
-					//refine ICP test
-					bool hasConverged;
-					double fitness;
-					inliers2 = util3d::transformPointCloud(inliers2, transform);
-					Transform icpT = util3d::icp(inliers1, <-- must be all correspondences, not only unique
-						inliers2,
-						0.02,
-						100,
-						hasConverged,
-						fitness);
-
-					transform = transform * icpT;
-					*/
-
 							if(inliers < this->getMinInliers())
 							{
 								transform.setNull();
@@ -382,6 +432,16 @@ Transform OdometryBOW::computeTransform(
 							UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
 						}
 					}
+					else
+					{
+						UWARN("Not enough 3D features in the new image (%d < %d)", (int)newSignature->getWords3().size(), this->getMinInliers());
+					}
+				}
+			}
+			else
+			{
+				UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), this->getMinInliers());
+			}
 
 			if(transform.isNull())
 			{
@@ -635,7 +695,6 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 
 		if(ki && ki >= this->getMinInliers())
 		{
-
 			std::vector<unsigned char> statusLast;
 			std::vector<float> errLast;
 			std::vector<cv::Point2f> lastCornersKeptRight;
@@ -650,6 +709,125 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
 						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
 
+			if(this->isPnPEstimationUsed())
+			{
+				// find correspondences
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners.resize(statusLast.size());
+					info->newCorners.resize(statusLast.size());
+				}
+
+				int flowInliers = 0;
+				std::vector<cv::Point3f> objectPoints(statusLast.size());
+				std::vector<cv::Point2f> imagePoints(statusLast.size());
+				int oi=0;
+				for(unsigned int i=0; i<statusLast.size(); ++i)
+				{
+					if(statusLast[i])
+					{
+						float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
+						float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
+						if(lastDisparity > 0.0f && lastSlope < stereoMaxSlope_)
+						{
+							pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
+									lastCornersKept[i],
+									lastDisparity,
+									data.cx(), data.cy(), data.fx(), data.baseline());
+
+							if(pcl::isFinite(lastPt3D) &&
+							   (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())))
+							{
+								//Add 3D correspondences!
+								lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
+								objectPoints[oi].x = lastPt3D.x;
+								objectPoints[oi].y = lastPt3D.y;
+								objectPoints[oi].z = lastPt3D.z;
+								imagePoints[oi] = newCornersKept.at(i);
+
+								if(this->isInfoDataFilled() && info)
+								{
+									info->refCorners[oi].pt = lastCornersKept[i];
+									info->newCorners[oi].pt = newCornersKept[i];
+								}
+								++oi;
+							}
+						}
+						++flowInliers;
+					}
+				}
+				objectPoints.resize(oi);
+				imagePoints.resize(oi);
+				UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
+
+				if(this->isInfoDataFilled() && info)
+				{
+					info->refCorners.resize(oi);
+					info->newCorners.resize(oi);
+				}
+
+				correspondences = oi;
+
+				if(correspondences >= this->getMinInliers())
+				{
+					//PnPRansac
+					cv::Mat K = (cv::Mat_<double>(3,3) <<
+						data.fx(), 0, data.cx(),
+						0, data.fyOrBaseline(), data.cy(),
+						0, 0, 1);
+					Transform guess = (data.localTransform()).inverse();
+					cv::Mat R = (cv::Mat_<double>(3,3) <<
+							(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
+							(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
+							(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
+					cv::Mat rvec(1,3, CV_64FC1);
+					cv::Rodrigues(R, rvec);
+					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
+					std::vector<int> inliersV;
+					cv::solvePnPRansac(objectPoints,
+							imagePoints,
+							K,
+							cv::Mat(),
+							rvec,
+							tvec,
+							true,
+							this->getIterations(),
+							this->getPnPReprojError(),
+							this->getMinInliers(),
+							inliersV,
+							this->getPnPFlags());
+
+					inliers = (int)inliersV.size();
+					if((int)inliersV.size() >= this->getMinInliers())
+					{
+						cv::Rodrigues(rvec, R);
+						Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
+									   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
+									   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
+
+						// make it incremental
+						output = (data.localTransform() * pnp).inverse();
+
+						UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
+
+						if(this->isInfoDataFilled() && info && inliersV.size())
+						{
+							info->cornerInliers = inliersV;
+						}
+					}
+					else
+					{
+						UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
+					}
+				}
+				else
+				{
+					UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
+				}
+			}
+			else
+			{
+
 				UDEBUG("");
 				std::vector<unsigned char> statusNew;
 				std::vector<float> errNew;
@@ -757,6 +935,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 				}
 			}
 		}
+	}
 	else
 	{
 		//return Identity
@@ -854,7 +1033,9 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 	}
 
 	std::vector<cv::Point2f> newCorners;
-	if(!refFrame_.empty() && refCorners_.size() && refCorners3D_->size())
+	if(!refFrame_.empty() &&
+		(int)refCorners_.size() >= this->getMinInliers() &&
+		(int)refCorners3D_->size() >= this->getMinInliers())
 	{
 		std::vector<unsigned char> status;
 		std::vector<float> err;
@@ -871,6 +1052,114 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
 		UDEBUG("cv::calcOpticalFlowPyrLK() end");
 
+		if(this->isPnPEstimationUsed())
+		{
+			// find correspondences
+			if(this->isInfoDataFilled() && info)
+			{
+				info->refCorners.resize(refCorners_.size());
+				info->newCorners.resize(refCorners_.size());
+			}
+
+			UASSERT(refCorners_.size() == refCorners3D_->size());
+			UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
+			int flowInliers = 0;
+			std::vector<cv::Point3f> objectPoints(refCorners_.size());
+			std::vector<cv::Point2f> imagePoints(refCorners_.size());
+			int oi=0;
+			for(unsigned int i=0; i<status.size(); ++i)
+			{
+				if(status[i])
+				{
+					if(pcl::isFinite(refCorners3D_->at(i)))
+					{
+						objectPoints[oi].x = refCorners3D_->at(i).x;
+						objectPoints[oi].y = refCorners3D_->at(i).y;
+						objectPoints[oi].z = refCorners3D_->at(i).z;
+						imagePoints[oi] = newCorners.at(i);
+
+						if(this->isInfoDataFilled() && info)
+						{
+							info->refCorners[oi].pt = refCorners_[i];
+							info->newCorners[oi].pt = newCorners[i];
+						}
+
+						++oi;
+					}
+					++flowInliers;
+				}
+			}
+			objectPoints.resize(oi);
+			imagePoints.resize(oi);
+			UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
+
+			if(this->isInfoDataFilled() && info)
+			{
+				info->refCorners.resize(oi);
+				info->newCorners.resize(oi);
+			}
+
+			correspondences = oi;
+
+			if(correspondences >= this->getMinInliers())
+			{
+				//PnPRansac
+				cv::Mat K = (cv::Mat_<double>(3,3) <<
+					data.fx(), 0, data.cx(),
+					0, data.fyOrBaseline(), data.cy(),
+					0, 0, 1);
+				Transform guess = (data.localTransform()).inverse();
+				cv::Mat R = (cv::Mat_<double>(3,3) <<
+						(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
+						(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
+						(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
+				cv::Mat rvec(1,3, CV_64FC1);
+				cv::Rodrigues(R, rvec);
+				cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
+				std::vector<int> inliersV;
+				cv::solvePnPRansac(objectPoints,
+						imagePoints,
+						K,
+						cv::Mat(),
+						rvec,
+						tvec,
+						true,
+						this->getIterations(),
+						this->getPnPReprojError(),
+						this->getMinInliers(),
+						inliersV,
+						this->getPnPFlags());
+
+				inliers = (int)inliersV.size();
+				if((int)inliersV.size() >= this->getMinInliers())
+				{
+					cv::Rodrigues(rvec, R);
+					Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
+								   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
+								   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
+
+					// make it incremental
+					output = (data.localTransform() * pnp).inverse();
+
+					UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
+
+					if(this->isInfoDataFilled() && info && inliersV.size())
+					{
+						info->cornerInliers = inliersV;
+					}
+				}
+				else
+				{
+					UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
+				}
+			}
+			else
+			{
+				UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
+			}
+		}
+		else
+		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
 			correspondencesLast->resize(refCorners_.size());
@@ -885,7 +1174,6 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 
 			UASSERT(refCorners_.size() == refCorners3D_->size());
 			UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
-		float sumSqrdDistance = 0.0f;
 			int flowInliers = 0;
 			for(unsigned int i=0; i<status.size(); ++i)
 			{
@@ -905,9 +1193,6 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 						correspondencesLast->at(oi) = refCorners3D_->at(i);
 						correspondencesNew->at(oi) = pt;
 
-					cv::Point2f diff = newCorners[i]-refCorners_[i];
-					sumSqrdDistance += diff.x*diff.x + diff.y*diff.y;
-
 						if(this->isInfoDataFilled() && info)
 						{
 							info->refCorners[oi].pt = refCorners_[i];
@@ -957,23 +1242,13 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 				{
 					info->cornerInliers = inliersV;
 				}
-
-			/*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, newFrame, newKpts,
-						   good_matches, imgMatches_, cv::Scalar::all(-1), cv::Scalar::all(-1),
-						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );*/
 			}
 			else
 			{
 				UWARN("Not enough correspondences (%d)", correspondences);
 			}
 		}
+	}
 	else
 	{
 		//return Identity

guilib/include/rtabmap/gui/ImageView.h

@@ -62,6 +62,7 @@ public:
 	int getAlpha() const {return _alpha;}
 	bool isGraphicsViewMode() const;
 	bool isGraphicsViewScaled() const;
+	const QColor & getBackgroundColor() const;
 
 	float viewScale() const;
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -287,6 +287,7 @@ private:
 	Transform _odometryCorrection;
 	Transform _lastOdomPose;
 	bool _processingOdometry;
+	double _lastOdomInfoUpdateTime;
 
 	QTimer * _oneSecondTimer;
 	QTime * _elapsedTime;

guilib/src/GuiLib.qrc

@@ -23,5 +23,9 @@
         <file>images/document-save.png</file>
         <file>images/document-properties.png</file>
         <file>images/system-log-out.png</file>
+        <file>images/kinect_xbox_360.png</file>
+        <file>images/kinect_xbox_one.png</file>
+        <file>images/sense.png</file>
+        <file>images/xtion_pro_live.png</file>
     </qresource>
 </RCC>

guilib/src/ImageView.cpp

@@ -148,6 +148,12 @@ bool ImageView::isGraphicsViewScaled() const
 	return _graphicsViewScaled->isChecked();
 }
 
+const QColor & ImageView::getBackgroundColor() const
+{
+	return _graphicsView->backgroundBrush().color();
+}
+
+
 void ImageView::setFeaturesShown(bool shown)
 {
 	_showFeatures->setChecked(shown);

guilib/src/MainWindow.cpp

@@ -76,6 +76,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QProcess>
 #include <QSplashScreen>
 #include <QInputDialog>
+#include <QToolButton>
 
 //RGB-D stuff
 #include "rtabmap/core/CameraRGBD.h"
@@ -127,6 +128,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_odomImageDepthShow(false),
 	_odometryCorrection(Transform::getIdentity()),
 	_processingOdometry(false),
+	_lastOdomInfoUpdateTime(0),
 	_oneSecondTimer(0),
 	_elapsedTime(0),
 	_posteriorCurve(0),
@@ -265,7 +267,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
-	_ui->toolBar->setWindowTitle(tr("Control toolbar"));
+	_ui->toolBar->setWindowTitle(tr("File toolbar"));
+	_ui->menuShow_view->addAction(_ui->toolBar_2->toggleViewAction());
+	_ui->toolBar_2->setWindowTitle(tr("Control toolbar"));
 	QAction * a = _ui->menuShow_view->addAction("Progress dialog");
 	a->setCheckable(false);
 	connect(a, SIGNAL(triggered(bool)), _initProgressDialog, SLOT(show()));
@@ -321,6 +325,13 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionReset_Odometry->setEnabled(false);
 	_ui->actionPost_processing->setEnabled(false);
 
+	QToolButton* toolButton = new QToolButton(this);
+	toolButton->setMenu(_ui->menuRGB_D_camera);
+	toolButton->setPopupMode(QToolButton::InstantPopup);
+	toolButton->setIcon(QIcon(":images/kinect_xbox_360.png"));
+	toolButton->setToolTip("Select sensor driver");
+	_ui->toolBar->addWidget(toolButton)->setObjectName("toolbar_source");
+
 #if defined(Q_WS_MAC) || defined(Q_WS_WIN)
 	connect(_ui->actionOpen_working_directory, SIGNAL(triggered()), SLOT(openWorkingDirectory()));
 #else
@@ -632,6 +643,10 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	}
 	else if(anEvent->getClassName().compare("OdometryEvent") == 0)
 	{
+		// limit 10 Hz max
+		if(UTimer::now() - _lastOdomInfoUpdateTime > 0.1)
+		{
+			_lastOdomInfoUpdateTime = UTimer::now();
 			OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
 			if(!_processingOdometry && !_processingStatistics)
 			{
@@ -639,6 +654,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 				emit odometryReceived(odomEvent->data(), odomEvent->info());
 			}
 		}
+	}
 	else if(anEvent->getClassName().compare("ULogEvent") == 0)
 	{
 		ULogEvent * logEvent = (ULogEvent*)anEvent;
@@ -4960,7 +4976,75 @@ void MainWindow::setMonitoringState(bool pauseChecked)
 // Must be called by the GUI thread, use signal StateChanged()
 void MainWindow::changeState(MainWindow::State newState)
 {
-	// TODO : To protect with mutex ?
+	bool monitoring = newState==kMonitoring || newState == kMonitoringPaused;
+	_ui->actionNew_database->setVisible(!monitoring);
+	_ui->actionOpen_database->setVisible(!monitoring);
+	_ui->actionClose_database->setVisible(!monitoring);
+	_ui->actionEdit_database->setVisible(!monitoring);
+	_ui->actionStart->setVisible(!monitoring);
+	_ui->actionStop->setVisible(!monitoring);
+	_ui->actionDump_the_memory->setVisible(!monitoring);
+	_ui->actionDump_the_prediction_matrix->setVisible(!monitoring);
+	_ui->actionGenerate_map->setVisible(!monitoring);
+	_ui->actionGenerate_local_map->setVisible(!monitoring);
+	_ui->actionGenerate_TORO_graph_graph->setVisible(!monitoring);
+	_ui->actionOpen_working_directory->setVisible(!monitoring);
+	_ui->actionData_recorder->setVisible(!monitoring);
+	_ui->menuSelect_source->menuAction()->setVisible(!monitoring);
+	_ui->doubleSpinBox_stats_imgRate->setVisible(!monitoring);
+	_ui->doubleSpinBox_stats_imgRate_label->setVisible(!monitoring);
+	_ui->toolBar->setVisible(!monitoring);
+	_ui->toolBar->toggleViewAction()->setVisible(!monitoring);
+	QList<QAction*> actions = _ui->menuTools->actions();
+	for(int i=0; i<actions.size(); ++i)
+	{
+		if(actions.at(i)->isSeparator())
+		{
+			actions.at(i)->setVisible(!monitoring);
+		}
+	}
+	actions = _ui->menuFile->actions();
+	if(actions.size()>=9)
+	{
+		if(actions.at(2)->isSeparator())
+		{
+			actions.at(2)->setVisible(!monitoring);
+		}
+		else
+		{
+			UWARN("Menu File separators have not the same order.");
+		}
+		if(actions.at(8)->isSeparator())
+		{
+			actions.at(8)->setVisible(!monitoring);
+		}
+		else
+		{
+			UWARN("Menu File separators have not the same order.");
+		}
+	}
+	else
+	{
+		UWARN("Menu File separators have not the same order.");
+	}
+	actions = _ui->menuProcess->actions();
+	if(actions.size()>=2)
+	{
+		if(actions.at(1)->isSeparator())
+		{
+			actions.at(1)->setVisible(!monitoring);
+		}
+		else
+		{
+			UWARN("Menu File separators have not the same order.");
+		}
+	}
+	else
+	{
+		UWARN("Menu File separators have not the same order.");
+	}
+
+
 	switch (newState)
 	{
 	case kIdle: // RTAB-Map is not initialized yet
@@ -4983,10 +5067,10 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionGenerate_map->setEnabled(false);
 		_ui->actionGenerate_local_map->setEnabled(false);
 		_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
-		_ui->actionOpen_working_directory->setEnabled(true);
 		_ui->actionDownload_all_clouds->setEnabled(false);
 		_ui->actionDownload_graph->setEnabled(false);
 		_ui->menuSelect_source->setEnabled(false);
+		_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
 		_ui->actionTrigger_a_new_map->setEnabled(false);
 		_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
 		_ui->statusbar->clearMessage();
@@ -5030,10 +5114,10 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionGenerate_map->setEnabled(true);
 		_ui->actionGenerate_local_map->setEnabled(true);
 		_ui->actionGenerate_TORO_graph_graph->setEnabled(true);
-		_ui->actionOpen_working_directory->setEnabled(true);
 		_ui->actionDownload_all_clouds->setEnabled(true);
 		_ui->actionDownload_graph->setEnabled(true);
 		_ui->menuSelect_source->setEnabled(true);
+		_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(true);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
 		_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
 		_ui->statusbar->clearMessage();
@@ -5066,10 +5150,10 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionGenerate_map->setEnabled(false);
 		_ui->actionGenerate_local_map->setEnabled(false);
 		_ui->actionGenerate_TORO_graph_graph->setEnabled(false);
-		_ui->actionOpen_working_directory->setEnabled(true);
 		_ui->actionDownload_all_clouds->setEnabled(false);
 		_ui->actionDownload_graph->setEnabled(false);
 		_ui->menuSelect_source->setEnabled(false);
+		_ui->toolBar->findChild<QAction*>("toolbar_source")->setEnabled(false);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
 		_ui->doubleSpinBox_stats_imgRate->setEnabled(true);
 		_ui->statusbar->showMessage(tr("Detecting..."));
@@ -5150,34 +5234,18 @@ void MainWindow::changeState(MainWindow::State newState)
 		}
 		break;
 	case kMonitoring:
-		_ui->actionNew_database->setVisible(false);
-		_ui->actionOpen_database->setVisible(false);
-		_ui->actionClose_database->setVisible(false);
-		_ui->actionEdit_database->setVisible(false);
-		_ui->actionStart->setVisible(false);
 		_ui->actionPause->setEnabled(true);
 		_ui->actionPause->setChecked(false);
 		_ui->actionPause->setToolTip(tr("Pause"));
-		_ui->actionStop->setVisible(false);
 		_ui->actionPause_on_match->setEnabled(true);
 		_ui->actionPause_on_local_loop_detection->setEnabled(true);
 		_ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
 		_ui->actionReset_Odometry->setEnabled(true);
 		_ui->actionPost_processing->setEnabled(false);
-		_ui->actionDump_the_memory->setVisible(false);
-		_ui->actionDump_the_prediction_matrix->setVisible(false);
 		_ui->actionDelete_memory->setEnabled(true);
-		_ui->actionGenerate_map->setVisible(false);
-		_ui->actionGenerate_local_map->setVisible(false);
-		_ui->actionGenerate_TORO_graph_graph->setVisible(false);
-		_ui->actionData_recorder->setVisible(false);
-		_ui->actionOpen_working_directory->setEnabled(false);
 		_ui->actionDownload_all_clouds->setEnabled(true);
 		_ui->actionDownload_graph->setEnabled(true);
-		_ui->menuSelect_source->setVisible(false);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
-		_ui->doubleSpinBox_stats_imgRate->setVisible(false);
-		_ui->doubleSpinBox_stats_imgRate_label->setVisible(false);
 		_ui->statusbar->showMessage(tr("Monitoring..."));
 		_state = newState;
 		_elapsedTime->start();
@@ -5185,34 +5253,18 @@ void MainWindow::changeState(MainWindow::State newState)
 		this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdPause, "", 0));
 		break;
 	case kMonitoringPaused:
-		_ui->actionNew_database->setVisible(false);
-		_ui->actionOpen_database->setVisible(false);
-		_ui->actionClose_database->setVisible(false);
-		_ui->actionEdit_database->setVisible(false);
-		_ui->actionStart->setVisible(false);
 		_ui->actionPause->setToolTip(tr("Continue"));
 		_ui->actionPause->setChecked(true);
 		_ui->actionPause->setEnabled(true);
-		_ui->actionStop->setVisible(false);
 		_ui->actionPause_on_match->setEnabled(true);
 		_ui->actionPause_on_local_loop_detection->setEnabled(true);
 		_ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
 		_ui->actionReset_Odometry->setEnabled(true);
 		_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
-		_ui->actionDump_the_memory->setVisible(false);
-		_ui->actionDump_the_prediction_matrix->setVisible(false);
 		_ui->actionDelete_memory->setEnabled(true);
-		_ui->actionGenerate_map->setVisible(false);
-		_ui->actionGenerate_local_map->setVisible(false);
-		_ui->actionGenerate_TORO_graph_graph->setVisible(false);
-		_ui->actionData_recorder->setVisible(false);
-		_ui->actionOpen_working_directory->setEnabled(false);
 		_ui->actionDownload_all_clouds->setEnabled(true);
 		_ui->actionDownload_graph->setEnabled(true);
-		_ui->menuSelect_source->setVisible(false);
 		_ui->actionTrigger_a_new_map->setEnabled(true);
-		_ui->doubleSpinBox_stats_imgRate->setVisible(false);
-		_ui->doubleSpinBox_stats_imgRate_label->setVisible(false);
 		_ui->statusbar->showMessage(tr("Monitoring paused..."));
 		_state = newState;
 		_oneSecondTimer->stop();

guilib/src/PreferencesDialog.cpp

@@ -522,6 +522,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_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());
+	_ui->odom_pnpEstimation->setObjectName(Parameters::kOdomPnPEstimation().c_str());
+	_ui->odom_pnpReprojError->setObjectName(Parameters::kOdomPnPReprojError().c_str());
+	_ui->odom_pnpFlags->setObjectName(Parameters::kOdomPnPFlags().c_str());
 
 	//Odometry BOW
 	_ui->odom_localHistory->setObjectName(Parameters::kOdomBowLocalHistorySize().c_str());

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -106,6 +106,10 @@
        <property name="title">
         <string>Kinect</string>
        </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/kinect_xbox_360.png</normaloff>:/images/kinect_xbox_360.png</iconset>
+       </property>
        <addaction name="actionFreenect"/>
        <addaction name="actionOpenNI2_kinect"/>
        <addaction name="actionOpenNI_PCL"/>
@@ -115,6 +119,10 @@
        <property name="title">
         <string>Xtion PRO LIVE</string>
        </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/xtion_pro_live.png</normaloff>:/images/xtion_pro_live.png</iconset>
+       </property>
        <addaction name="actionOpenNI2"/>
        <addaction name="actionOpenNI_PCL_ASUS"/>
        <addaction name="actionOpenNI_CV_ASUS"/>
@@ -123,6 +131,10 @@
        <property name="title">
         <string>Sense 3D scanner</string>
        </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/sense.png</normaloff>:/images/sense.png</iconset>
+       </property>
        <addaction name="actionOpenNI2_Sense"/>
       </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
@@ -221,16 +233,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -249,13 +252,9 @@
    <attribute name="toolBarBreak">
     <bool>false</bool>
    </attribute>
+   <addaction name="actionClose_database"/>
    <addaction name="actionNew_database"/>
    <addaction name="actionOpen_database"/>
-   <addaction name="actionClose_database"/>
-   <addaction name="separator"/>
-   <addaction name="actionStart"/>
-   <addaction name="actionPause"/>
-   <addaction name="actionStop"/>
   </widget>
   <widget class="QDockWidget" name="dockWidget_statsV2">
    <property name="floating">
@@ -272,16 +271,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -524,16 +514,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -554,16 +535,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -584,16 +556,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -614,16 +577,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -644,16 +598,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -677,16 +622,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -707,16 +643,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -740,16 +667,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -819,16 +737,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -837,6 +746,20 @@
     </layout>
    </widget>
   </widget>
+  <widget class="QToolBar" name="toolBar_2">
+   <property name="windowTitle">
+    <string>toolBar_2</string>
+   </property>
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+   <addaction name="actionStart"/>
+   <addaction name="actionPause"/>
+   <addaction name="actionStop"/>
+  </widget>
   <action name="actionExit">
    <property name="icon">
     <iconset resource="../GuiLib.qrc">

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>1</number>
+                <number>23</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -6610,6 +6610,81 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                          </property>
                         </widget>
                        </item>
+                       <item row="5" column="1">
+                        <widget class="QLabel" name="label_225">
+                         <property name="text">
+                          <string>PnP reprojection error.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="0">
+                        <widget class="QDoubleSpinBox" name="odom_pnpReprojError">
+                         <property name="suffix">
+                          <string> pix</string>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>8.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="0">
+                        <widget class="QCheckBox" name="odom_pnpEstimation">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="1">
+                        <widget class="QLabel" name="label_226">
+                         <property name="text">
+                          <string>PnP RANSAC: Pose estimation from 2D to 3D correspondences instead of 3D to 3D correspondences. PnP uses &quot;Minimum feature correspondences&quot; and &quot;Maximum iterations&quot; above.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="1">
+                        <widget class="QLabel" name="label_227">
+                         <property name="text">
+                          <string>PnP flags.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="0">
+                        <widget class="QComboBox" name="odom_pnpFlags">
+                         <item>
+                          <property name="text">
+                           <string>Iterative</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>EPNP</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>P3P</string>
+                          </property>
+                         </item>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>

tools/OdometryViewer/CMakeLists.txt

@@ -1,8 +1,4 @@
 
-SET(SRC_FILES
-    main.cpp
-)
-
 SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/corelib/include
 	${PROJECT_SOURCE_DIR}/utilite/include
@@ -20,6 +16,16 @@ SET(LIBRARIES
 	${QT_LIBRARIES} 
 )
 
+IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
+    QT4_WRAP_CPP(moc_srcs OdomInfoWidget.h)
+ELSE()
+    QT5_WRAP_CPP(moc_srcs OdomInfoWidget.h)
+ENDIF()
+
+SET(SRC_FILES
+    main.cpp OdomInfoWidget.cpp ${moc_srcs}
+)
+
 add_definitions(${PCL_DEFINITIONS})
 
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

tools/OdometryViewer/OdomInfoWidget.cpp

@@ -0,0 +1,208 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "OdomInfoWidget.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/gui/ImageView.h>
+#include <rtabmap/gui/UCv2Qt.h>
+#include <QtCore/QMetaType>
+#include <QtGui/QCloseEvent>
+#include <QLabel>
+#include <QVBoxLayout>
+
+OdomInfoWidget::OdomInfoWidget(QWidget * parent) :
+		QWidget(parent),
+	imageView_(new rtabmap::ImageView(this)),
+	label_(new QLabel(this)),
+	processingOdomInfo_(false),
+	receivingRate_(0),
+	lastTime_(0),
+	odomImageShow_(true),
+	odomImageDepthShow_(false)
+{
+	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
+	qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
+
+	imageView_->setMinimumSize(320, 240);
+	QVBoxLayout * layout = new QVBoxLayout(this);
+	layout->setMargin(0);
+	layout->addWidget(imageView_);
+	layout->addWidget(label_);
+	layout->setStretch(0,1);
+	this->setLayout(layout);
+}
+
+
+OdomInfoWidget::~OdomInfoWidget()
+{
+	this->unregisterFromEventsManager();
+}
+
+
+void OdomInfoWidget::processOdomInfo(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
+{
+	processingOdomInfo_ = true;
+
+	const rtabmap::Transform & pose = data.pose();
+	bool lost = false;
+	bool lostStateChanged = false;
+
+	if(pose.isNull())
+	{
+		// lost
+		lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::darkRed);
+		lost = true;
+	}
+	else
+	{
+		// ok
+		lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
+		imageView_->setBackgroundColor(Qt::black);
+	}
+
+	if(!data.image().empty())
+	{
+		if(imageView_->isFeaturesShown())
+		{
+			if(info.type == 0)
+			{
+				imageView_->setFeatures(info.words, Qt::yellow);
+			}
+			else if(info.type == 1)
+			{
+				imageView_->setFeatures(info.refCorners, Qt::red);
+			}
+		}
+
+		imageView_->clearLines();
+		if(lost)
+		{
+			if(lostStateChanged)
+			{
+				// save state
+				odomImageShow_ = imageView_->isImageShown();
+				odomImageDepthShow_ = imageView_->isImageDepthShown();
+			}
+			imageView_->setImageDepth(uCvMat2QImage(data.image()));
+			imageView_->setImageShown(true);
+			imageView_->setImageDepthShown(true);
+		}
+		else
+		{
+			if(lostStateChanged)
+			{
+				// restore state
+				imageView_->setImageShown(odomImageShow_);
+				imageView_->setImageDepthShown(odomImageDepthShow_);
+			}
+
+			imageView_->setImage(uCvMat2QImage(data.image()));
+			if(imageView_->isImageDepthShown())
+			{
+				imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
+			}
+
+			if(info.type == 0)
+			{
+				if(imageView_->isFeaturesShown())
+				{
+					for(unsigned int i=0; i<info.wordMatches.size(); ++i)
+					{
+						imageView_->setFeatureColor(info.wordMatches[i], Qt::red); // outliers
+					}
+					for(unsigned int i=0; i<info.wordInliers.size(); ++i)
+					{
+						imageView_->setFeatureColor(info.wordInliers[i], Qt::green); // inliers
+					}
+				}
+			}
+			else if(info.type == 1)
+			{
+				if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
+				{
+					//draw lines
+					UASSERT(info.refCorners.size() == info.newCorners.size());
+					for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
+					{
+						if(imageView_->isFeaturesShown())
+						{
+							imageView_->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
+						}
+						if(imageView_->isLinesShown())
+						{
+							imageView_->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,
+									Qt::blue);
+						}
+					}
+					imageView_->update();
+				}
+			}
+
+		}
+		if(!data.image().empty())
+		{
+			imageView_->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
+		}
+	}
+	label_->setText(tr("Rate=~%1 Hz").arg(receivingRate_));
+	processingOdomInfo_ = false;
+}
+
+void OdomInfoWidget::closeEvent(QCloseEvent* event)
+{
+	this->unregisterFromEventsManager();
+	event->accept();
+}
+
+void OdomInfoWidget::handleEvent(UEvent * event)
+{
+	if(event->getClassName().compare("OdometryEvent") == 0)
+	{
+		rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
+		receivingRate_ = 1.0f/timer_.ticks();
+
+		// update max 10 Hz
+		if(UTimer::now() - lastTime_ > 0.1)
+		{
+			lastTime_ = UTimer::now();
+			if(!processingOdomInfo_ && this->isVisible())
+			{
+				QMetaObject::invokeMethod(this, "processOdomInfo",
+						Q_ARG(rtabmap::SensorData, odomEvent->data()),
+						Q_ARG(rtabmap::OdometryInfo, odomEvent->info()));
+			}
+		}
+	}
+}

tools/OdometryViewer/OdomInfoWidget.h

@@ -0,0 +1,66 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMINFOWIDGET_H_
+#define ODOMINFOWIDGET_H_
+
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <QWidget>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/OdometryInfo.h>
+
+class QLabel;
+
+namespace rtabmap{
+class ImageView;
+}
+
+class OdomInfoWidget : public QWidget, public UEventsHandler
+{
+	Q_OBJECT
+public:
+	OdomInfoWidget(QWidget * parent = 0);
+	virtual ~OdomInfoWidget();
+public slots:
+	void processOdomInfo(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info);
+protected:
+	virtual void closeEvent(QCloseEvent* event);
+	void handleEvent(UEvent * event);
+
+private:
+	rtabmap::ImageView* imageView_;
+	QLabel* label_;
+	UTimer timer_;
+	bool processingOdomInfo_;
+	double receivingRate_;
+	double lastTime_;
+	bool odomImageShow_;
+	bool odomImageDepthShow_;
+};
+
+#endif /* ODOMINFOWIDGET_H_ */

tools/OdometryViewer/main.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/VWDictionary.h>
 #include <QApplication>
 #include <pcl/console/print.h>
+#include "OdomInfoWidget.h"
 
 void showUsage()
 {
@@ -679,17 +680,22 @@ int main (int argc, char * argv[])
 	}
 	rtabmap::OdometryThread odomThread(odom);
 	rtabmap::OdometryViewer odomViewer(maxClouds, 2, 0.0, 50);
+	OdomInfoWidget odomInfoWidget;
 	UEventsManager::addHandler(&odomThread);
 	UEventsManager::addHandler(&odomViewer);
+	UEventsManager::addHandler(&odomInfoWidget);
 
 	odomViewer.setCameraFree();
 	odomViewer.setGridShown(true);
 
-	odomViewer.setWindowTitle("Odometry viewer");
+	odomViewer.setWindowTitle("Odometry 3D view");
 	odomViewer.setMinimumWidth(800);
 	odomViewer.setMinimumHeight(500);
 	odomViewer.showNormal();
 
+	odomInfoWidget.setWindowTitle("Odometry info");
+	odomInfoWidget.showNormal();
+
 	app.processEvents();
 
 	if(inputDatabase.size())