Added Vis/PnPVarianceMedianRatio parameter (to tune pnp computed cova…

…riance)

corelib/include/rtabmap/core/Parameters.h

@@ -597,6 +597,7 @@ class RTABMAP_CORE_EXPORT Parameters
 #else
     RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #endif
+    RTABMAP_PARAM(Vis, PnPVarianceMedianRatio,   int, 4,        uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, PnPMaxVariance,           float, 0.0,    uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, PnPSamplingPolicy,      unsigned int, 1, uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str());
 

corelib/include/rtabmap/core/RegistrationVis.h

@@ -82,6 +82,7 @@ class RTABMAP_CORE_EXPORT RegistrationVis : public Registration
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;
+	int  _PnPVarMedianRatio;
 	float _PnPMaxVar;
 	unsigned int _multiSamplingPolicy;
 	int _correspondencesApproach;

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -48,6 +48,7 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
+			int varianceMedianRatio = 4,
 			float maxVariance = 0,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
@@ -65,6 +66,7 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
+			int varianceMedianRatio = 4,
 			float maxVariance = 0,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),

corelib/src/RegistrationVis.cpp

@@ -69,6 +69,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
+		_PnPVarMedianRatio(Parameters::defaultVisPnPVarianceMedianRatio()),
 		_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
 		_multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()),
 		_correspondencesApproach(Parameters::defaultVisCorType()),
@@ -126,6 +127,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
 	Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
+	Parameters::parse(parameters, Parameters::kVisPnPVarianceMedianRatio(), _PnPVarMedianRatio);
 	Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
 	Parameters::parse(parameters, Parameters::kVisPnPSamplingPolicy(), _multiSamplingPolicy);
 	Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
@@ -1586,6 +1588,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									_PnPReprojError,
 									_PnPFlags,
 									_PnPRefineIterations,
+									_PnPVarMedianRatio,
 									_PnPMaxVar,
 									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 									words3B,
@@ -1608,6 +1611,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									_PnPReprojError,
 									_PnPFlags,
 									_PnPRefineIterations,
+									_PnPVarMedianRatio,
 									_PnPMaxVar,
 									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
 									words3B,

corelib/src/util3d_motion_estimation.cpp

@@ -64,6 +64,7 @@ Transform estimateMotion3DTo2D(
 			double reprojError,
 			int flagsPnP,
 			int refineIterations,
+			int varianceMedianRatio,
 			float maxVariance,
 			const Transform & guess,
 			const std::map<int, cv::Point3f> & words3B,
@@ -73,6 +74,7 @@ Transform estimateMotion3DTo2D(
 {
 	UASSERT(cameraModel.isValidForProjection());
 	UASSERT(!guess.isNull());
+	UASSERT(varianceMedianRatio>1);
 	Transform transform;
 	std::vector<int> matches, inliers;
 
@@ -194,11 +196,11 @@ Transform estimateMotion3DTo2D(
 
 				std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
 				//divide by 4 instead of 2 to ignore very very far features (stereo)
-				double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+				double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
 				UASSERT(uIsFinite(median_error_sqr_lin));
 				(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
 				std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
-				double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
+				double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
 				UASSERT(uIsFinite(median_error_sqr_ang));
 				(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
 
@@ -251,6 +253,7 @@ Transform estimateMotion3DTo2D(
 			double reprojError,
 			int flagsPnP,
 			int refineIterations,
+			int varianceMedianRatio,
 			float maxVariance,
 			const Transform & guess,
 			const std::map<int, cv::Point3f> & words3B,
@@ -271,6 +274,7 @@ Transform estimateMotion3DTo2D(
 	}
 
 	UASSERT(!guess.isNull());
+	UASSERT(varianceMedianRatio > 1);
 
 	std::vector<int> matches, inliers;
 
@@ -530,11 +534,11 @@ Transform estimateMotion3DTo2D(
 
 				std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
 				//divide by 4 instead of 2 to ignore very very far features (stereo)
-				double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
+				double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
 				UASSERT(uIsFinite(median_error_sqr_lin));
 				(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
 				std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
-				double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
+				double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
 				UASSERT(uIsFinite(median_error_sqr_ang));
 				(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
 

guilib/src/PreferencesDialog.cpp

@@ -1174,6 +1174,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
 	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
+	_ui->loopClosure_pnpVarMedianRatio->setObjectName(Parameters::kVisPnPVarianceMedianRatio().c_str());
 	_ui->loopClosure_pnpMaxVariance->setObjectName(Parameters::kVisPnPMaxVariance().c_str());
 	_ui->loopClosure_pnpSamplingPolicy->setObjectName(Parameters::kVisPnPSamplingPolicy().c_str());
 	_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());