i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Vis/PnPVarianceMedianRatio parameter (to tune pnp computed covariance)

Browse Source
This commit is contained in:
matlabbe
2023-07-27 16:13:30 -07:00
parent 446590b19f
commit 424dc90dce
7 changed files with 149 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/RegistrationVis.cpp
View File

@@ -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,

12
corelib/src/util3d_motion_estimation.cpp
View File

@@ -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;