Fixed PnP in OpenCV3: added parameter Vis/PnPRefineIterations (default 1)

CameraRGBDImages: Fixed calibration not loaded
Fixed loading ground truth from RGBD-SLAM format
MainWindow: added ground truth paths in CloudViewer and GraphViewer

corelib/include/rtabmap/core/Odometry.h

@@ -66,8 +66,8 @@ public:
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	int getEstimationType() const {return _estimationType;}
 	double getPnPReprojError() const {return _pnpReprojError;}
-	int  getPnPFlags() const {return _pnpFlags;}
+	int getPnPFlags() const {return _pnpFlags;}
-	bool  getPnPOpenCV2() const {return _pnpOpenCV2;}
+	int getPnPRefineIterations() const {return _pnpRefineIterations;}
 	const Transform & previousTransform() const {return previousTransform_;}
 	bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
 
@@ -98,7 +98,7 @@ private:
 	int _estimationType;
 	double _pnpReprojError;
 	int _pnpFlags;
-	bool _pnpOpenCV2;
+	int _pnpRefineIterations;
 	bool _varianceFromInliersCount;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;

corelib/include/rtabmap/core/Parameters.h

@@ -364,12 +364,12 @@ class RTABMAP_EXP Parameters
 	
 	// Visual registration parameters
 	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
-	RTABMAP_PARAM(Vis, ForwardEstOnly,               bool, true, 	"Forward estimation (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
+	RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true, 	"Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
 	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Vis, RefineIterations,         int, 10,       "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(Vis, PnPReprojError, 	         double, 5.0,   "[Vis/EstimationType = 1] PnP reprojection error.");
 	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
-	RTABMAP_PARAM(Vis, PnPOpenCV2,               bool, true,    "[Vis/EstimationType = 1] Use OpenCV2 solvePnPRansac() in OpenCV3.");
+	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
 	RTABMAP_PARAM(Vis, MinInliers,               int, 10, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");

corelib/include/rtabmap/core/RegistrationVis.h

@@ -67,7 +67,7 @@ private:
 	bool _forwardEstimateOnly;
 	double _PnPReprojError;
 	int _PnPFlags;
-	bool _PnPOpenCV2;
+	bool _PnPRefineIterations;
 	int _correspondencesApproach;
 	int _flowWinSize;
 	int _flowIterations;

corelib/include/rtabmap/core/util3d_features.h

@@ -73,7 +73,7 @@ std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
 		int pnpIterations = 100,
 		float pnpReprojError = 8.0f,
 		int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
-		bool pnpOpenCV2 = true,
+		int pnpRefineIterations = 1,
 		float ransacParam1 = 3.0f,
 		float ransacParam2 = 0.99f,
 		const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -47,7 +47,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
 			int iterations = 100,
 			double reprojError = 5.,
 			int flagsPnP = 0,
-			bool pnpOpenCV2 = true,
+			int pnpRefineIterations = 1,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
 			double * varianceOut = 0, // mean reproj error if words3B is not set
@@ -66,19 +66,20 @@ Transform RTABMAP_EXP estimateMotion3DTo3D(
 			std::vector<int> * inliersOut = 0);
 
 void RTABMAP_EXP solvePnPRansac(
-		cv::InputArray _opoints,
+		const std::vector<cv::Point3f> & objectPoints,
-		cv::InputArray _ipoints,
+		const std::vector<cv::Point2f> & imagePoints,
-		cv::InputArray _cameraMatrix,
+		const cv::Mat & cameraMatrix,
-		cv::InputArray _distCoeffs,
+		const cv::Mat & distCoeffs,
-		cv::OutputArray _rvec,
+		cv::Mat & rvec,
-		cv::OutputArray _tvec,
+		cv::Mat & tvec,
 		bool useExtrinsicGuess,
-        int iterationsCount,
+		int iterationsCount,
-        float reprojectionError,
+		float reprojectionError,
-        int minInliersCount,
+		int minInliersCount,
-        cv::OutputArray _inliers,
+		std::vector<int> & inliers,
-        int flags,
+		int flags,
-        bool opencv2version);
+		int refineIterations = 1,
+		float refineSigma = 3.0f);
 
 } // namespace util3d
 } // namespace rtabmap

corelib/include/rtabmap/core/util3d_registration.h

@@ -50,9 +50,8 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
 		double inlierThreshold = 0.02,
 		int iterations = 100,
-		bool refineModel = false,
-		double refineModelSigma = 3.0,
 		int refineModelIterations = 10,
+		double refineModelSigma = 3.0,
 		std::vector<int> * inliers = 0,
 		double * variance = 0);
 

corelib/src/CameraRGB.cpp

@@ -174,6 +174,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	}
 
 	// look for calibration files
+	UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
 		if(!_model.load(calibrationFolder, cameraName))
@@ -285,7 +286,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			else if(_groundTruthFormat == 1)
 			{
 				//Match ground truth values with images
-				groundTruth_.resize(stamps_.size(), Transform());
+				groundTruth_.clear();
 				std::map<double, int> stampsToIds;
 				for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
 				{
@@ -293,6 +294,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 				}
 				std::vector<double> values = uValues(stamps);
 
+				Transform firstPoseInv;
 				for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
 				{
 					Transform pose; // null transform
@@ -319,7 +321,18 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 								pose = ta.interpolate(t, tb);
 							}
 						}
-
+					}
+					if(!pose.isNull())
+					{
+						if(firstPoseInv.isNull())
+						{
+							firstPoseInv = pose.inverse();
+							pose.setIdentity();
+						}
+						else
+						{
+							pose = firstPoseInv * pose;
+						}
 					}
 					groundTruth_.push_back(pose);
 				}
@@ -328,7 +341,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			{
 				groundTruth_ = uValuesList(poses);
 			}
-			UASSERT(groundTruth_.size() == stamps_.size());
+			UASSERT_MSG(groundTruth_.size() == stamps_.size(), uFormat("%d vs %d", (int)groundTruth_.size(), (int)stamps_.size()).c_str());
 		}
 	}
 

corelib/src/CameraRGBD.cpp

@@ -1690,7 +1690,7 @@ CameraRGBDImages::~CameraRGBDImages()
 bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	bool success = false;
-	if(CameraImages::init() && cameraDepth_.init())
+	if(CameraImages::init(calibrationFolder, cameraName) && cameraDepth_.init())
 	{
 		if(this->imagesCount() == cameraDepth_.imagesCount())
 		{

corelib/src/Graph.cpp

@@ -202,7 +202,7 @@ bool importPoses(
 				std::list<std::string> strList = uSplit(str);
 				if(strList.size() ==  8)
 				{
-					double stamp = uStr2Float(strList.front());
+					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
 					str = uJoin(strList, " ");
 					Transform pose = Transform::fromString(str);
@@ -214,6 +214,12 @@ bool importPoses(
 					{
 						stamps->insert(std::make_pair(id, stamp));
 					}
+					// we need to remove optical rotation
+					// z pointing front, x left, y down
+					Transform t( 0, 0, 1, 0,
+								-1, 0, 0, 0,
+								 0,-1, 0, 0);
+					pose = t * pose * t.inverse();
 					poses.insert(std::make_pair(id, pose));
 				}
 				else

corelib/src/Memory.cpp

@@ -2040,7 +2040,7 @@ Transform Memory::computeVisualTransform(
 			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
 			transform = _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
 		}
-		else
+		else if(fromS->getWords().size() && toS->getWords().size())
 		{
 			transform = _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
 		}
@@ -3310,6 +3310,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
 		s->sensorData().setUserDataRaw(data.userDataRaw());
 	}
+	s->sensorData().setGroundTruth(data.groundTruth());
 
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

corelib/src/Odometry.cpp

@@ -55,7 +55,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_estimationType(Parameters::defaultVisEstimationType()),
 		_pnpReprojError(Parameters::defaultVisPnPReprojError()),
 		_pnpFlags(Parameters::defaultVisPnPFlags()),
-		_pnpOpenCV2(Parameters::defaultVisPnPOpenCV2()),
+		_pnpRefineIterations(Parameters::defaultVisPnPRefineIterations()),
 		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
 		_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
 		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
@@ -79,7 +79,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
-	Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _pnpOpenCV2);
+	Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _pnpRefineIterations);
 	UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
 	Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
 	Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);

corelib/src/OdometryBOW.cpp

@@ -257,7 +257,7 @@ Transform OdometryBOW::computeTransform(
 								this->getIterations(),
 								this->getPnPReprojError(),
 								this->getPnPFlags(),
-								this->getPnPOpenCV2(),
+								this->getPnPRefineIterations(),
 								this->getPose(),
 								uMultimapToMap(newSignature->getWords3()),
 								isVarianceFromInliersCount()?0:&variance, // don't compute variance if we use inliers

corelib/src/OdometryF2F.cpp

@@ -84,7 +84,7 @@ Transform OdometryF2F::computeTransform(
 		output = registration_.computeTransformationMod(
 				refFrame_,
 				newFrame,
-				guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform::getIdentity(),
+				guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
 				&rejectedMsg,
 				&inliers,
 				&variance);

corelib/src/OdometryMono.cpp

@@ -368,7 +368,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 								0, // min inliers
 								inliersV,
 								this->getPnPFlags(),
-								this->getPnPOpenCV2());
+								this->getPnPRefineIterations());
 
 						UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
 
@@ -434,7 +434,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 										this->getIterations(),
 										this->getPnPReprojError(),
 										this->getPnPFlags(),
-										this->getPnPOpenCV2(),
+										this->getPnPRefineIterations(),
 										fundMatrixReprojError_,
 										fundMatrixConfidence_,
 										previousGuess,
@@ -897,7 +897,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 										0, // min inliers
 										inliersPnP,
 										this->getPnPFlags(),
-										this->getPnPOpenCV2());
+										this->getPnPRefineIterations());
 
 								UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
 

corelib/src/RegistrationVis.cpp

@@ -52,7 +52,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
 		_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
-		_PnPOpenCV2(Parameters::defaultVisPnPOpenCV2()),
+		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
 		_correspondencesApproach(Parameters::defaultVisCorType()),
 		_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -96,7 +96,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
-	Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _PnPOpenCV2);
+	Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
 	Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
 	Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -557,7 +557,7 @@ Transform RegistrationVis::computeTransformationMod(
 							_iterations,
 							_PnPReprojError,
 							_PnPFlags, // cv::SOLVEPNP_ITERATIVE
-							_PnPOpenCV2,
+							_PnPRefineIterations,
 							1.0f,
 							0.99f,
 							uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
@@ -635,7 +635,7 @@ Transform RegistrationVis::computeTransformationMod(
 								_iterations,
 								_PnPReprojError,
 								_PnPFlags,
-								_PnPOpenCV2,
+								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 								uMultimapToMapUnique(signatureA->getWords3()),
 								_varianceFromInliersCount?0:&variances[dir],
@@ -718,16 +718,39 @@ Transform RegistrationVis::computeTransformationMod(
 			}
 			else
 			{
-				transform = transforms[0].interpolate(0.5f, transforms[1]);
+				/*if(!guess.isNull())
-				if(inliersOut)
 				{
-					*inliersOut = inliers[0];
+					// use the transform nearest of the guess
+					int index = 0;
+					if(transforms[0].getDistance(guess) > transforms[1].getDistance(guess))
+					{
+						index = 1;
+					}
+					transform = transforms[index];
+					if(inliersOut)
+					{
+						*inliersOut = inliers[index];
+					}
+
+					variance = variances[index];
+					if(_varianceFromInliersCount)
+					{
+						variance = inliers[index].size() > 0?1.0f/float(inliers[index].size()):1.0f;
+					}
 				}
-				variance = (variances[0]+variances[1])/2.0f;
+				else*/
-				if(_varianceFromInliersCount)
 				{
-					int avg = (inliers[0].size()+inliers[1].size())/2;
+					transform = transforms[0].interpolate(0.5f, transforms[1]);
-					variance = avg>0?1.0f/float(avg):1.0f;
+					if(inliersOut)
+					{
+						*inliersOut = inliers[0];
+					}
+					variance = (variances[0]+variances[1])/2.0f;
+					if(_varianceFromInliersCount)
+					{
+						int avg = (inliers[0].size()+inliers[1].size())/2;
+						variance = avg>0?1.0f/float(avg):1.0f;
+					}
 				}
 			}
 		}

corelib/src/util2d.cpp

@@ -167,7 +167,6 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 		int tmpMaxDisparity = maxDisparity;
 
 		int iterations = 0;
-		std::vector<float> scores;
 		for(int level=maxLevel; level>=0; --level)
 		{
 			UASSERT(level < (int)leftPyramid.size());
@@ -199,7 +198,13 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 					localMinDisparity += maxCol-leftPyramid[level].cols-1;
 				}
 
-				scores = std::vector<float>(localMinDisparity-localMaxDisparity+1, 0.0f);
+				if(localMinDisparity < localMaxDisparity)
+				{
+					localMaxDisparity = localMinDisparity;
+				}
+				int length = localMinDisparity-localMaxDisparity+1;
+				std::vector<float> scores = std::vector<float>(length, 0.0f);
+
 				for(int d=localMinDisparity; d>localMaxDisparity; --d)
 				{
 					++iterations;
@@ -220,6 +225,7 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 				{
 					if(level>0)
 					{
+						UDEBUG("");
 						tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
 						tmpMaxDisparity+=tmpMaxDisparity%level;
 						if(tmpMaxDisparity > maxDisparity)
@@ -241,6 +247,7 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 
 		if(bestScoreIndex>=0)
 		{
+			UDEBUG("");
 			//subpixel refining
 			int d = -(tmpMinDisparity+bestScoreIndex);
 
@@ -314,21 +321,20 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 				}
 			}
 
-			if(leftCorners[i].x+float(d) == xc)
-			{
-				++noSubPixel;
-			}
-
 			rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
 			status[i] = reject?0:1;
 			if(!reject)
 			{
+				if(leftCorners[i].x+float(d) != xc)
+				{
+					++noSubPixel;
+				}
 				++added;
 			}
 		}
 		subpixelTime+=timer.ticks();
 	}
-	UDEBUG("noSubPixel=%d/%d", noSubPixel, added);
+	UDEBUG("SubPixel=%d/%d added (total=%d)", noSubPixel, added, (int)status.size());
 	UDEBUG("totalIterations=%d", totalIterations);
 	UDEBUG("Time pyramid = %f s", pyramidTime);
 	UDEBUG("Time disparity = %f s", disparityTime);

corelib/src/util3d_features.cpp

@@ -183,7 +183,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 		int pnpIterations,
 		float pnpReprojError,
 		int pnpFlags,
-		bool pnpOpenCV2,
+		int pnpRefineIterations,
 		float ransacParam1,
 		float ransacParam2,
 		const std::map<int, cv::Point3f> & refGuess3D,
@@ -401,7 +401,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 										0, // min inliers
 										inliersV,
 										pnpFlags,
-										pnpOpenCV2);
+										pnpRefineIterations);
 
 								UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
 

corelib/src/util3d_motion_estimation.cpp

@@ -49,7 +49,7 @@ Transform estimateMotion3DTo2D(
 			int iterations,
 			double reprojError,
 			int flagsPnP,
-			bool pnpOpenCV2,
+			int refineIterations,
 			const Transform & guess,
 			const std::map<int, cv::Point3f> & words3B,
 			double * varianceOut,
@@ -94,6 +94,7 @@ Transform estimateMotion3DTo2D(
 	{
 		//PnPRansac
 		cv::Mat K = cameraModel.K();
+		cv::Mat D = cameraModel.D();
 		Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse();
 		cv::Mat R = (cv::Mat_<double>(3,3) <<
 				(double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(),
@@ -109,7 +110,7 @@ Transform estimateMotion3DTo2D(
 				objectPoints,
 				imagePoints,
 				K,
-				cv::Mat(),
+				D,
 				rvec,
 				tvec,
 				true,
@@ -118,7 +119,7 @@ Transform estimateMotion3DTo2D(
 				0, // min inliers
 				inliers,
 				flagsPnP,
-				pnpOpenCV2);
+				refineIterations);
 
 		if((int)inliers.size() >= minInliers)
 		{
@@ -234,9 +235,8 @@ Transform estimateMotion3DTo3D(
 				inliers1cloud,
 				inliersDistance,
 				iterations,
-				refineIterations>0,
-				3.0,
 				refineIterations,
+				3.0,
 				&inliers,
 				varianceOut);
 
@@ -262,329 +262,181 @@ Transform estimateMotion3DTo3D(
 	return transform;
 }
 
-// Don't know why, but the RANSAC implementation in OpenCV 3 gives me far
+
-// more wrong results than the 2.4x implementation.
+std::vector<float> computeReprojErrors(
-// Here is a copy of the RANSAC implementation from OpenCV 2.4.x version
+		std::vector<cv::Point3f> opoints,
-#if CV_MAJOR_VERSION >= 3
+		std::vector<cv::Point2f> ipoints,
-namespace pnpransac
+		const cv::Mat & cameraMatrix,
+		const cv::Mat & distCoeffs,
+		const cv::Mat & rvec,
+		const cv::Mat & tvec,
+		float reprojErrorThreshold,
+		std::vector<int> & inliers)
 {
-	const int MIN_POINTS_COUNT = 4;
+	UASSERT(opoints.size() == ipoints.size());
+	int count = (int)opoints.size();
 
-	static void project3dPoints(const cv::Mat& points, const cv::Mat& rvec, const cv::Mat& tvec, cv::Mat& modif_points)
+	std::vector<cv::Point2f> projpoints;
+	projectPoints(opoints, rvec, tvec, cameraMatrix, distCoeffs, projpoints);
+
+	inliers.resize(count,0);
+	std::vector<float> err(count);
+	int oi=0;
+	for (int i = 0; i < count; ++i)
 	{
-		modif_points.create(1, points.cols, CV_32FC3);
+		float e = (float)cv::norm( ipoints[i] - projpoints[i]);
-		cv::Mat R(3, 3, CV_64FC1);
+		if(e <= reprojErrorThreshold)
-		cv::Rodrigues(rvec, R);
-		cv::Mat transformation(3, 4, CV_64F);
-		cv::Mat r = transformation.colRange(0, 3);
-		R.copyTo(r);
-		cv::Mat t = transformation.colRange(3, 4);
-		tvec.copyTo(t);
-		transform(points, modif_points, transformation);
-	}
-
-	struct CameraParameters
-	{
-		void init(cv::Mat _intrinsics, cv::Mat _distCoeffs)
 		{
-			_intrinsics.copyTo(intrinsics);
+			inliers[oi] = i;
-			_distCoeffs.copyTo(distortion);
+			err[oi++] = e;
-		}
-
-		cv::Mat intrinsics;
-		cv::Mat distortion;
-	};
-
-	struct Parameters
-	{
-		int iterationsCount;
-		float reprojectionError;
-		int minInliersCount;
-		bool useExtrinsicGuess;
-		int flags;
-		CameraParameters camera;
-	};
-
-	template <typename OpointType, typename IpointType>
-	static void pnpTask(const int curIndex, const std::vector<char>& pointsMask, const cv::Mat& objectPoints, const cv::Mat& imagePoints,
-				 const Parameters& params, std::vector<int>& inliers, int& bestIndex, cv::Mat& rvec, cv::Mat& tvec,
-				 const cv::Mat& rvecInit, const cv::Mat& tvecInit, cv::Mutex& resultsMutex)
-	{
-		cv::Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(cv::DataDepth<OpointType>::value, 3));
-		cv::Mat modelImagePoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(cv::DataDepth<IpointType>::value, 2));
-		for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
-		{
-			if (pointsMask[i])
-			{
-				cv::Mat colModelImagePoints = modelImagePoints(cv::Rect(colIndex, 0, 1, 1));
-				imagePoints.col(i).copyTo(colModelImagePoints);
-				cv::Mat colModelObjectPoints = modelObjectPoints(cv::Rect(colIndex, 0, 1, 1));
-				objectPoints.col(i).copyTo(colModelObjectPoints);
-				colIndex = colIndex+1;
-			}
-		}
-
-		//filter same 3d points, hang in solvePnP
-		double eps = 1e-10;
-		int num_same_points = 0;
-		for (int i = 0; i < MIN_POINTS_COUNT; i++)
-			for (int j = i + 1; j < MIN_POINTS_COUNT; j++)
-			{
-				if (norm(modelObjectPoints.at<cv::Vec<OpointType,3> >(0, i) - modelObjectPoints.at<cv::Vec<OpointType,3> >(0, j)) < eps)
-					num_same_points++;
-			}
-		if (num_same_points > 0)
-			return;
-
-		cv::Mat localRvec, localTvec;
-		rvecInit.copyTo(localRvec);
-		tvecInit.copyTo(localTvec);
-
-		// OpenCV 3
-		cv::solvePnP(
-				modelObjectPoints,
-				modelImagePoints,
-				params.camera.intrinsics,
-				params.camera.distortion,
-				localRvec,
-				localTvec,
-				 params.useExtrinsicGuess,
-				 params.flags);
-
-
-		std::vector<cv::Point_<OpointType> > projected_points;
-		projected_points.resize(objectPoints.cols);
-		projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points);
-
-		cv::Mat rotatedPoints;
-		project3dPoints(objectPoints, localRvec, localTvec, rotatedPoints);
-
-		std::vector<int> localInliers;
-		for (int i = 0; i < objectPoints.cols; i++)
-		{
-			//Although p is a 2D point it needs the same type as the object points to enable the norm calculation
-			cv::Point_<OpointType> p((OpointType)imagePoints.at<cv::Vec<IpointType,2> >(0, i)[0],
-								 (OpointType)imagePoints.at<cv::Vec<IpointType,2> >(0, i)[1]);
-			if ((norm(p - projected_points[i]) < params.reprojectionError)
-				&& (rotatedPoints.at<cv::Vec<OpointType,3> >(0, i)[2] > 0)) //hack
-			{
-				localInliers.push_back(i);
-			}
-		}
-
-		resultsMutex.lock();
-		if ( (localInliers.size() > inliers.size()) || (localInliers.size() == inliers.size() && curIndex > bestIndex))
-		{
-			inliers.clear();
-			inliers.resize(localInliers.size());
-			memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
-			localRvec.copyTo(rvec);
-			localTvec.copyTo(tvec);
-			bestIndex = curIndex;
-		}
-		resultsMutex.unlock();
-	}
-
-	static void pnpTask(const int curIndex, const std::vector<char>& pointsMask, const cv::Mat& objectPoints, const cv::Mat& imagePoints,
-		const Parameters& params, std::vector<int>& inliers, int& bestIndex, cv::Mat& rvec, cv::Mat& tvec,
-		const cv::Mat& rvecInit, const cv::Mat& tvecInit, cv::Mutex& resultsMutex)
-	{
-		CV_Assert(objectPoints.depth() == CV_64F ||  objectPoints.depth() == CV_32F);
-		CV_Assert(imagePoints.depth() == CV_64F ||  imagePoints.depth() == CV_32F);
-		const bool objectDoublePrecision = objectPoints.depth() == CV_64F;
-		const bool imageDoublePrecision = imagePoints.depth() == CV_64F;
-		if(objectDoublePrecision)
-		{
-			if(imageDoublePrecision)
-				pnpTask<double, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
-			else
-				pnpTask<double, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
-		}
-		else
-		{
-			if(imageDoublePrecision)
-				pnpTask<float, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
-			else
-				pnpTask<float, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
 		}
 	}
-
+	inliers.resize(oi);
-	// TBB removed
+	err.resize(oi);
-	class PnPSolver
+	return err;
-	{
-	public:
-		void operator()(int begin, int end) const
-		{
-			std::vector<char> pointsMask(objectPoints.cols, 0);
-			for( int i=begin; i!=end; ++i )
-			{
-				memset(&pointsMask[0], 0, objectPoints.cols );
-				memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
-				generateVar(pointsMask, rng_base_seed + i);
-				pnpTask(i, pointsMask, objectPoints, imagePoints, parameters,
-						inliers, bestIndex, rvec, tvec, initRvec, initTvec, syncMutex);
-				if ((int)inliers.size() >= parameters.minInliersCount)
-				{
-					break;
-				}
-			}
-		}
-		PnPSolver(const cv::Mat& _objectPoints, const cv::Mat& _imagePoints, const Parameters& _parameters,
-				cv::Mat& _rvec, cv::Mat& _tvec, std::vector<int>& _inliers, int& _bestIndex, uint64 _rng_base_seed):
-		objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
-		rvec(_rvec), tvec(_tvec), inliers(_inliers), bestIndex(_bestIndex), rng_base_seed(_rng_base_seed)
-		{
-			bestIndex = -1;
-			rvec.copyTo(initRvec);
-			tvec.copyTo(initTvec);
-		}
-	private:
-		PnPSolver& operator=(const PnPSolver&);
-
-		const cv::Mat& objectPoints;
-		const cv::Mat& imagePoints;
-		const Parameters& parameters;
-		cv::Mat &rvec, &tvec;
-		std::vector<int>& inliers;
-		int& bestIndex;
-		const uint64 rng_base_seed;
-		cv::Mat initRvec, initTvec;
-
-		static cv::Mutex syncMutex;
-
-	  void generateVar(std::vector<char>& mask, uint64 rng_seed) const
-		{
-		    cv::RNG generator(rng_seed);
-			int size = (int)mask.size();
-			for (int i = 0; i < size; i++)
-			{
-				int i1 = generator.uniform(0, size);
-				int i2 = generator.uniform(0, size);
-				char curr = mask[i1];
-				mask[i1] = mask[i2];
-				mask[i2] = curr;
-			}
-		}
-	};
-
-	cv::Mutex PnPSolver::syncMutex;
-
 }
-#endif
 
 void solvePnPRansac(
-		cv::InputArray _opoints,
+		const std::vector<cv::Point3f> & objectPoints,
-		cv::InputArray _ipoints,
+		const std::vector<cv::Point2f> & imagePoints,
-		cv::InputArray _cameraMatrix,
+		const cv::Mat & cameraMatrix,
-		cv::InputArray _distCoeffs,
+		const cv::Mat & distCoeffs,
-		cv::OutputArray _rvec,
+		cv::Mat & rvec,
-		cv::OutputArray _tvec,
+		cv::Mat & tvec,
 		bool useExtrinsicGuess,
         int iterationsCount,
         float reprojectionError,
         int minInliersCount,
-        cv::OutputArray _inliers,
+        std::vector<int> & inliers,
         int flags,
-        bool opencv2version)
+        int refineIterations,
+        float refineSigma)
 {
-#if CV_MAJOR_VERSION >= 3
+	cv::solvePnPRansac(
-	if(opencv2version)
+			objectPoints,
-	{
+			imagePoints,
-		const int _rng_seed = 0;
+			cameraMatrix,
-		cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
+			distCoeffs,
-		cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
+			rvec,
-
+			tvec,
-		CV_Assert(opoints.isContinuous());
+			useExtrinsicGuess,
-		CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
+			iterationsCount,
-		CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
+			reprojectionError,
-		CV_Assert(ipoints.isContinuous());
-		CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
-		CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
-
-		_rvec.create(3, 1, CV_64FC1);
-		_tvec.create(3, 1, CV_64FC1);
-		cv::Mat rvec = _rvec.getMat();
-		cv::Mat tvec = _tvec.getMat();
-
-		cv::Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
-
-		if (minInliersCount <= 0)
-			minInliersCount = objectPoints.cols;
-		pnpransac::Parameters params;
-		params.iterationsCount = iterationsCount;
-		params.minInliersCount = minInliersCount;
-		params.reprojectionError = reprojectionError;
-		params.useExtrinsicGuess = useExtrinsicGuess;
-		params.camera.init(cameraMatrix, distCoeffs);
-		params.flags = flags;
-
-		std::vector<int> localInliers;
-		cv::Mat localRvec, localTvec;
-		rvec.copyTo(localRvec);
-		tvec.copyTo(localTvec);
-		int bestIndex;
-
-		// TBB not used
-		if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
-		{
-			pnpransac::PnPSolver solver(objectPoints, imagePoints, params,
-								   localRvec, localTvec, localInliers, bestIndex,
-								   _rng_seed);
-			solver(0, iterationsCount);
-		}
-
-		if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
-		{
-			if (flags != CV_P3P)
-			{
-				int i, pointsCount = (int)localInliers.size();
-				cv::Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
-				for (i = 0; i < pointsCount; i++)
-				{
-					int index = localInliers[i];
-					cv::Mat colInlierImagePoints = inlierImagePoints(cv::Rect(i, 0, 1, 1));
-					imagePoints.col(index).copyTo(colInlierImagePoints);
-					cv::Mat colInlierObjectPoints = inlierObjectPoints(cv::Rect(i, 0, 1, 1));
-					objectPoints.col(index).copyTo(colInlierObjectPoints);
-				}
-				solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags);
-			}
-			localRvec.copyTo(rvec);
-			localTvec.copyTo(tvec);
-			if (_inliers.needed())
-				cv::Mat(localInliers).copyTo(_inliers);
-		}
-		else
-		{
-			tvec.setTo(cv::Scalar(0));
-			cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
-			Rodrigues(R, rvec);
-			if( _inliers.needed() )
-				_inliers.release();
-		}
-	}
-	else
-#endif
-	{
-		cv::solvePnPRansac(
-				_opoints,
-				_ipoints,
-				_cameraMatrix,
-				_distCoeffs,
-				_rvec,
-				_tvec,
-				useExtrinsicGuess,
-				iterationsCount,
-				reprojectionError,
 #if CV_MAJOR_VERSION < 3
-				minInliersCount, // min inliers
+			minInliersCount, // min inliers
 #else
-				0.99, // confidence
+			0.99, // confidence
 #endif
-				_inliers,
+			inliers,
-				flags);
+			flags);
+
+	float inlierThreshold = reprojectionError;
+	if(inliers.size() >= 4 && refineIterations>0)
+	{
+		float inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
+		float error_threshold = inlierThreshold;
+		float sigma_sqr = refineSigma * refineSigma;
+		int refine_iterations = 0;
+		bool inlier_changed = false, oscillating = false;
+		std::vector<int> new_inliers, prev_inliers = inliers;
+		std::vector<size_t> inliers_sizes;
+		//Eigen::VectorXf new_model_coefficients = model_coefficients;
+		cv::Mat new_model_rvec = rvec;
+		cv::Mat new_model_tvec = tvec;
+
+		do
+		{
+			// Get inliers from the current model
+			std::vector<cv::Point3f> opoints_inliers(prev_inliers.size());
+			std::vector<cv::Point2f> ipoints_inliers(prev_inliers.size());
+			for(unsigned int i=0; i<prev_inliers.size(); ++i)
+			{
+				opoints_inliers[i] = objectPoints[prev_inliers[i]];
+				ipoints_inliers[i] = imagePoints[prev_inliers[i]];
+			}
+
+			UDEBUG("inliers=%d refine_iterations=%d, rvec=%f,%f,%f tvec=%f,%f,%f", (int)prev_inliers.size(), refine_iterations,
+					*new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
+					*new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));
+
+			// Optimize the model coefficients
+			cv::solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, true, flags);
+			inliers_sizes.push_back(prev_inliers.size());
+
+			UDEBUG("rvec=%f,%f,%f tvec=%f,%f,%f",
+					*new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
+					*new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));
+
+			// Select the new inliers based on the optimized coefficients and new threshold
+			std::vector<float> err = computeReprojErrors(objectPoints, imagePoints, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, error_threshold, new_inliers);
+			UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
+					(int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
+
+			if (new_inliers.size() < 4)
+			{
+				++refine_iterations;
+				if (refine_iterations >= refineIterations)
+				{
+					break;
+				}
+				continue;
+			}
+
+			// Estimate the variance and the new threshold
+			float m = uMean(err.data(), err.size());
+			float variance = uVariance(err.data(), err.size());
+			error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
+
+			UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f mean=%f) on iteration %d out of %d.",
+				  error_threshold, variance, m, refine_iterations, refineIterations);
+			inlier_changed = false;
+			std::swap (prev_inliers, new_inliers);
+
+			// If the number of inliers changed, then we are still optimizing
+			if (new_inliers.size () != prev_inliers.size ())
+			{
+				// Check if the number of inliers is oscillating in between two values
+				if (inliers_sizes.size () >= 4)
+				{
+					if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
+					inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
+					{
+						oscillating = true;
+						break;
+					}
+				}
+				inlier_changed = true;
+				continue;
+			}
+
+			// Check the values of the inlier set
+			for (size_t i = 0; i < prev_inliers.size (); ++i)
+			{
+				// If the value of the inliers changed, then we are still optimizing
+				if (prev_inliers[i] != new_inliers[i])
+				{
+					inlier_changed = true;
+					break;
+				}
+			}
+		}
+		while (inlier_changed && ++refine_iterations < refineIterations);
+
+		// If the new set of inliers is empty, we didn't do a good job refining
+		if (new_inliers.empty ())
+		{
+			UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
+		}
+
+		if (oscillating)
+		{
+			UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
+		}
+
+		std::swap (inliers, new_inliers);
+		rvec = new_model_rvec;
+		tvec = new_model_tvec;
 	}
-    return;
 
 }
 

corelib/src/util3d_registration.cpp

@@ -49,9 +49,8 @@ Transform transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
 		double inlierThreshold,
 		int iterations,
-		bool refineModel,
+		int refineIterations,
-		double refineModelSigma,
+		double refineSigma,
-		int refineModelIterations,
 		std::vector<int> * inliersOut,
 		double * varianceOut)
 {
@@ -97,11 +96,11 @@ Transform transformFromXYZCorrespondences(
 			sac.getInliers(inliers);
 			sac.getModelCoefficients (model_coefficients);
 
-			if (refineModel)
+			if (refineIterations>0)
 			{
 				double inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
 				double error_threshold = inlierThreshold;
-				double sigma_sqr = refineModelSigma * refineModelSigma;
+				double sigma_sqr = refineSigma * refineSigma;
 				int refine_iterations = 0;
 				bool inlier_changed = false, oscillating = false;
 				std::vector<int> new_inliers, prev_inliers = inliers;
@@ -121,7 +120,7 @@ Transform transformFromXYZCorrespondences(
 					if (new_inliers.empty ())
 					{
 						++refine_iterations;
-						if (refine_iterations >= refineModelIterations)
+						if (refine_iterations >= refineIterations)
 						{
 							break;
 						}
@@ -133,7 +132,7 @@ Transform transformFromXYZCorrespondences(
 					error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
 
 					UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
-						  error_threshold, variance, refine_iterations, refineModelIterations);
+						  error_threshold, variance, refine_iterations, refineIterations);
 					inlier_changed = false;
 					std::swap (prev_inliers, new_inliers);
 
@@ -165,7 +164,7 @@ Transform transformFromXYZCorrespondences(
 						}
 					}
 				}
-				while (inlier_changed && ++refine_iterations < refineModelIterations);
+				while (inlier_changed && ++refine_iterations < refineIterations);
 
 				// If the new set of inliers is empty, we didn't do a good job refining
 				if (new_inliers.empty ())

guilib/include/rtabmap/gui/GraphViewer.h

@@ -57,6 +57,7 @@ public:
 	void updateGraph(const std::map<int, Transform> & poses,
 					 const std::multimap<int, Link> & constraints,
 					 const std::map<int, int> & mapIds);
+	void updateGTGraph(const std::map<int, Transform> & poses);
 	void updateReferentialPosition(const Transform & t);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
 	void updatePosterior(const std::map<int, float> & posterior);
@@ -87,6 +88,7 @@ public:
 	const QColor & getRejectedLoopClosureColor() const {return _loopClosureRejectedColor;}
 	const QColor & getLocalPathColor() const {return _localPathColor;}
 	const QColor & getGlobalPathColor() const {return _globalPathColor;}
+	const QColor & getGTColor() const {return _gtPathColor;}
 	const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;}
 	const QColor & getInterSessionLoopColor() const {return _loopInterSessionColor;}
 	bool isIntraInterSessionColorsEnabled() const {return _intraInterSessionColors;}
@@ -112,6 +114,7 @@ public:
 	void setRejectedLoopClosureColor(const QColor & color);
 	void setLocalPathColor(const QColor & color);
 	void setGlobalPathColor(const QColor & color);
+	void setGTColor(const QColor & color);
 	void setIntraSessionLoopColor(const QColor & color);
 	void setInterSessionLoopColor(const QColor & color);
 	void setIntraInterSessionColorsEnabled(bool enabled);
@@ -145,12 +148,15 @@ private:
 	QColor _loopClosureRejectedColor;
 	QColor _localPathColor;
 	QColor _globalPathColor;
+	QColor _gtPathColor;
 	QColor _loopIntraSessionColor;
 	QColor _loopInterSessionColor;
 	bool _intraInterSessionColors;
 	QGraphicsItem * _root;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;
+	QMap<int, NodeItem*> _gtNodeItems;
+	QMultiMap<int, LinkItem*> _gtLinkItems;
 	QMultiMap<int, LinkItem*> _localPathLinkItems;
 	QMultiMap<int, LinkItem*> _globalPathLinkItems;
 	float _nodeRadius;

guilib/include/rtabmap/gui/MainWindow.h

@@ -292,6 +292,7 @@ private:
 
 	QMap<int, Signature> _cachedSignatures;
 	std::map<int, Transform> _currentPosesMap; // <nodeId, pose>
+	std::map<int, Transform> _currentGTPosesMap; // <nodeId, pose>
 	std::multimap<int, Link> _currentLinksMap; // <nodeFromId, link>
 	std::map<int, int> _currentMapIds;   // <nodeId, mapId>
 	std::map<int, std::string> _curentLabels; // <nodeId, label>

guilib/src/GraphViewer.cpp

@@ -65,6 +65,7 @@ public:
 		this->setBrush(pen().color());
 		this->setAcceptHoverEvents(true);
 	}
+	virtual ~NodeItem() {}
 
 	void setColor(const QColor & color)
 	{
@@ -114,6 +115,7 @@ public:
 	{
 		this->setAcceptHoverEvents(true);
 	}
+	virtual ~LinkItem() {}
 
 	void setColor(const QColor & color)
 	{
@@ -183,6 +185,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_loopClosureRejectedColor(Qt::black),
 		_localPathColor(Qt::cyan),
 		_globalPathColor(Qt::darkMagenta),
+		_gtPathColor(Qt::gray),
 		_loopIntraSessionColor(Qt::red),
 		_loopInterSessionColor(Qt::green),
 		_intraInterSessionColors(false),
@@ -459,6 +462,124 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 	UDEBUG("_nodeItems=%d, _linkItems=%d", _nodeItems.size(), _linkItems.size());
 }
 
+void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
+{
+	bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
+	UDEBUG("poses=%d", (int)poses.size());
+	//Hide nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
+	{
+		iter.value()->hide();
+		iter.value()->setColor(_gtPathColor); // reset color
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(!iter->second.isNull())
+		{
+			QMap<int, NodeItem*>::iterator itemIter = _gtNodeItems.find(iter->first);
+			if(itemIter != _gtNodeItems.end())
+			{
+				itemIter.value()->setPose(iter->second);
+				itemIter.value()->show();
+			}
+			else
+			{
+				// create node item
+				const Transform & pose = iter->second;
+				NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius);
+				this->scene()->addItem(item);
+				item->setZValue(20);
+				item->setColor(_gtPathColor);
+				item->setParentItem(_root);
+				_gtNodeItems.insert(iter->first, item);
+			}
+
+			if(iter!=poses.begin())
+			{
+				std::map<int, Transform>::const_iterator iterPrevious = iter;
+				--iterPrevious;
+				Transform previousPose = iterPrevious->second;
+				Transform currentPose = iter->second;
+
+				LinkItem * linkItem = 0;
+				QMultiMap<int, LinkItem*>::iterator linkIter = _gtLinkItems.end();
+				if(_gtLinkItems.contains(iterPrevious->first))
+				{
+					linkIter = _gtLinkItems.find(iter->first);
+					while(linkIter.key() == iterPrevious->first && linkIter != _gtLinkItems.end())
+					{
+						if(linkIter.value()->to() == iter->first)
+						{
+							linkIter.value()->setPoses(previousPose, currentPose);
+							linkIter.value()->show();
+							linkItem = linkIter.value();
+							break;
+						}
+						++linkIter;
+					}
+				}
+				if(linkItem == 0)
+				{
+					//create a link item
+					linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link::kUndef, 1);
+					QPen p = linkItem->pen();
+					p.setWidthF(_linkWidth);
+					linkItem->setPen(p);
+					linkItem->setZValue(10);
+					this->scene()->addItem(linkItem);
+					linkItem->setParentItem(_root);
+					_gtLinkItems.insert(iterPrevious->first, linkItem);
+				}
+				if(linkItem)
+				{
+					linkItem->setColor(_gtPathColor);
+				}
+			}
+		}
+	}
+
+	//remove not used nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _gtNodeItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _gtLinkItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
+	this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+
+	if(wasEmpty)
+	{
+		QRectF rect = this->scene()->itemsBoundingRect();
+		this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
+	}
+
+	UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d", _gtNodeItems.size(), _gtLinkItems.size());
+}
+
 void GraphViewer::updateReferentialPosition(const Transform & t)
 {
 	QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24(), -t.o14());
@@ -695,6 +816,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("rejected_color", this->getRejectedLoopClosureColor());
 	settings.setValue("local_path_color", this->getLocalPathColor());
 	settings.setValue("global_path_color", this->getGlobalPathColor());
+	settings.setValue("gt_color", this->getGTColor());
 	settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
 	settings.setValue("inter_session_color", this->getInterSessionLoopColor());
 	settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled());
@@ -729,6 +851,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value<QColor>());
 	this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
 	this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
+	this->setGTColor(settings.value("gt_color", this->getGTColor()).value<QColor>());
 	this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
 	this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value<QColor>());
 	this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
@@ -772,6 +895,10 @@ void GraphViewer::setNodeRadius(float radius)
 	{
 		iter.value()->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
 	}
+	for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
+	{
+		iter.value()->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
+	}
 }
 void GraphViewer::setLinkWidth(float width)
 {
@@ -887,6 +1014,18 @@ void GraphViewer::setGlobalPathColor(const QColor & color)
 {
 	_globalPathColor = color;
 }
+void GraphViewer::setGTColor(const QColor & color)
+{
+	_gtPathColor = color;
+	for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
+	{
+		iter.value()->setColor(_gtPathColor);
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter=_gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
+	{
+		iter.value()->setColor(_gtPathColor);
+	}
+}
 void GraphViewer::setIntraSessionLoopColor(const QColor & color)
 {
 	_loopIntraSessionColor = color;
@@ -1022,6 +1161,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold..."));
 	QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
 	QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
+	QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth"));
 	menuLink->addSeparator();
 	QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors"));
 	QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure"));
@@ -1035,6 +1175,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor));
 	aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
 	aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
+	aChangeGTColor->setIcon(createIcon(_gtPathColor));
 	aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
 	aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor));
 	aChangeNeighborColor->setIconVisibleInMenu(true);
@@ -1046,6 +1187,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aChangeRejectedLoopColor->setIconVisibleInMenu(true);
 	aChangeLocalPathColor->setIconVisibleInMenu(true);
 	aChangeGlobalPathColor->setIconVisibleInMenu(true);
+	aChangeGTColor->setIconVisibleInMenu(true);
 	aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
 	aChangeInterSessionLoopColor->setIconVisibleInMenu(true);
 	aSetIntraInterSessionColors->setCheckable(true);
@@ -1205,6 +1347,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			r == aChangeRejectedLoopColor ||
 			r == aChangeLocalPathColor ||
 			r == aChangeGlobalPathColor ||
+			r == aChangeGTColor ||
 			r == aChangeIntraSessionLoopColor ||
 			r == aChangeInterSessionLoopColor)
 	{
@@ -1249,6 +1392,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		{
 			color = _globalPathColor;
 		}
+		else if(r == aChangeGTColor)
+		{
+			color = _gtPathColor;
+		}
 		else if(r == aChangeIntraSessionLoopColor)
 		{
 			color = _loopIntraSessionColor;
@@ -1305,6 +1452,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			{
 				this->setGlobalPathColor(color);
 			}
+			else if(r == aChangeGTColor)
+			{
+				this->setGTColor(color);
+			}
 			else if(r == aChangeIntraSessionLoopColor)
 			{
 				this->setIntraSessionLoopColor(color);

guilib/src/MainWindow.cpp

@@ -1099,6 +1099,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	if(uContains(stat.getSignatures(), stat.refImageId()))
 	{
 		refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
+		if(!stat.getSignatures().at(stat.refImageId()).sensorData().groundTruth().isNull())
+		{
+			_currentGTPosesMap.insert(std::make_pair(stat.refImageId(), stat.getSignatures().at(stat.refImageId()).sensorData().groundTruth()));
+		}
 	}
 	int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
 	int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
@@ -1633,6 +1637,20 @@ void MainWindow::updateMapCloud(
 			kter->second->push_back(pt);
 		}
 
+		//Ground truth graph?
+		for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
+		{
+			int mapId = -100;
+			//edges
+			std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
+			if(kter == graphs.end())
+			{
+				kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
+			}
+			pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
+			kter->second->push_back(pt);
+		}
+
 		// add graphs
 		for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
 		{
@@ -1677,6 +1695,8 @@ void MainWindow::updateMapCloud(
 		{
 			_ui->graphicsView_graphView->updateReferentialPosition(currentPose);
 		}
+
+		_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
 	}
 	cv::Mat map8U;
 	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))
@@ -4078,6 +4098,7 @@ void MainWindow::clearTheCache()
 	_ui->widget_cloudViewer->clearTrajectory();
 	_ui->widget_mapVisibility->clear();
 	_currentPosesMap.clear();
+	_currentGTPosesMap.clear();
 	_currentLinksMap.clear();
 	_currentMapIds.clear();
 	_curentLabels.clear();

guilib/src/PreferencesDialog.cpp

@@ -208,11 +208,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
-#if CV_MAJOR_VERSION < 3
-	_ui->loopClosure_pnpOpenCV2->setVisible(false);
-	_ui->label_loopClosure_pnpOpenCV2->setVisible(false);
-#endif
-
 	// Default Driver
 	connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
@@ -649,7 +644,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
 	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
-	_ui->loopClosure_pnpOpenCV2->setObjectName(Parameters::kVisPnPOpenCV2().c_str());
+	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
 	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
 
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
@@ -683,6 +678,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
+	connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryType, SLOT(setCurrentIndex(int)));
+	_ui->odom_strategy->setCurrentIndex(Parameters::defaultOdomStrategy());
 	_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
 	_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
@@ -2522,7 +2519,7 @@ void PreferencesDialog::selectSourceRGBDImagesPathGt()
 		{
 			list.push_back(_ui->comboBox_cameraRGBDImages_gtFormat->itemText(i));
 		}
-		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list);
+		QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, 0, false);
 		if(!item.isEmpty())
 		{
 			_ui->lineEdit_cameraRGBDImages_gt->setText(path);
@@ -3945,6 +3942,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 				dir = d.absolutePath();
 			}
 		}
+
 		if(!camera->init(useRawImages?"":dir.toStdString(), name.toStdString()))
 		{
 			UWARN("init camera failed... ");