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;}
-	bool  getPnPOpenCV2() const {return _pnpOpenCV2;}
+	int getPnPFlags() const {return _pnpFlags;}
+	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,
-		cv::InputArray _ipoints,
-		cv::InputArray _cameraMatrix,
-		cv::InputArray _distCoeffs,
-		cv::OutputArray _rvec,
-		cv::OutputArray _tvec,
+		const std::vector<cv::Point3f> & objectPoints,
+		const std::vector<cv::Point2f> & imagePoints,
+		const cv::Mat & cameraMatrix,
+		const cv::Mat & distCoeffs,
+		cv::Mat & rvec,
+		cv::Mat & tvec,
 		bool useExtrinsicGuess,
-        int iterationsCount,
-        float reprojectionError,
-        int minInliersCount,
-        cv::OutputArray _inliers,
-        int flags,
-        bool opencv2version);
+		int iterationsCount,
+		float reprojectionError,
+		int minInliersCount,
+		std::vector<int> & inliers,
+		int flags,
+		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);