/*---------------------------------------------------------------
mahalanobisDistanceTo
---------------------------------------------------------------*/
double CPointPDFGaussian::mahalanobisDistanceTo( const CPointPDFGaussian & other, bool only_2D ) const
{
// The difference in means:
CMatrixDouble13 deltaX;
deltaX.get_unsafe(0,0) = other.mean.x() - mean.x();
deltaX.get_unsafe(0,1) = other.mean.y() - mean.y();
deltaX.get_unsafe(0,2) = other.mean.z() - mean.z();
// The inverse of the combined covs:
CMatrixDouble33 COV = other.cov;
COV += this->cov;
if (!only_2D)
{
CMatrixDouble33 COV_inv;
COV.inv(COV_inv);
return sqrt( deltaX.multiply_HCHt_scalar(COV_inv) );
}
else
{
CMatrixDouble22 C = COV.block(0,0,2,2);
CMatrixDouble22 COV_inv;
C.inv(COV_inv);
CMatrixDouble12 deltaX2 = deltaX.block(0,0,1,2);
return std::sqrt( deltaX2.multiply_HCHt_scalar(COV_inv) );
}
}
/*---------------------------------------------------------------
productIntegralWith
---------------------------------------------------------------*/
double CPointPDFGaussian::productIntegralWith( const CPointPDFGaussian &p) const
{
MRPT_START
// --------------------------------------------------------------
// 12/APR/2009 - Jose Luis Blanco:
// The integral over all the variable space of the product of two
// Gaussians variables amounts to simply the evaluation of
// a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
// ---------------------------------------------------------------
CMatrixDouble33 C = cov; C+=p.cov; // Sum of covs:
CMatrixDouble33 C_inv;
C.inv(C_inv);
CMatrixDouble31 MU(UNINITIALIZED_MATRIX); // Diff. of means
MU.get_unsafe(0,0) = mean.x() - p.mean.x();
MU.get_unsafe(1,0) = mean.y() - p.mean.y();
MU.get_unsafe(2,0) = mean.z() - p.mean.z();
return std::pow( M_2PI, -0.5*state_length )
* (1.0/std::sqrt( C.det() ))
* exp( -0.5* MU.multiply_HtCH_scalar(C_inv) );
MRPT_END
}
/*----------------------------------------------------------------------------
ICP_Method_Classic
----------------------------------------------------------------------------*/
CPosePDFPtr CICP::ICP_Method_Classic(
const mrpt::maps::CMetricMap *m1,
const mrpt::maps::CMetricMap *mm2,
const CPosePDFGaussian &initialEstimationPDF,
TReturnInfo &outInfo )
{
MRPT_START
// The result can be either a Gaussian or a SOG:
CPosePDFPtr resultPDF;
CPosePDFGaussianPtr gaussPdf;
CPosePDFSOGPtr SOG;
size_t nCorrespondences=0;
bool keepApproaching;
CPose2D grossEst = initialEstimationPDF.mean;
mrpt::utils::TMatchingPairList correspondences,old_correspondences;
CPose2D lastMeanPose;
// Assure the class of the maps:
const mrpt::maps::CMetricMap *m2 = mm2;
// Asserts:
// -----------------
ASSERT_( options.ALFA>=0 && options.ALFA<1 );
// The algorithm output auxiliar info:
// -------------------------------------------------
outInfo.nIterations = 0;
outInfo.goodness = 1;
outInfo.quality = 0;
// The gaussian PDF to estimate:
// ------------------------------------------------------
gaussPdf = CPosePDFGaussian::Create();
// First gross approximation:
gaussPdf->mean = grossEst;
// Initial thresholds:
mrpt::maps::TMatchingParams matchParams;
mrpt::maps::TMatchingExtraResults matchExtraResults;
matchParams.maxDistForCorrespondence = options.thresholdDist; // Distance threshold
matchParams.maxAngularDistForCorrespondence = options.thresholdAng; // Angular threshold
matchParams.onlyKeepTheClosest = options.onlyClosestCorrespondences;
matchParams.onlyUniqueRobust = options.onlyUniqueRobust;
matchParams.decimation_other_map_points = options.corresponding_points_decimation;
// Asure maps are not empty!
// ------------------------------------------------------
if ( !m2->isEmpty() )
{
matchParams.offset_other_map_points = 0;
// ------------------------------------------------------
// The ICP loop
// ------------------------------------------------------
do
{
// ------------------------------------------------------
// Find the matching (for a points map)
// ------------------------------------------------------
matchParams.angularDistPivotPoint = TPoint3D(gaussPdf->mean.x(),gaussPdf->mean.y(),0); // Pivot point for angular measurements
m1->determineMatching2D(
m2, // The other map
gaussPdf->mean, // The other map pose
correspondences,
matchParams, matchExtraResults);
nCorrespondences = correspondences.size();
// ***DEBUG***
// correspondences.dumpToFile("debug_correspondences.txt");
if ( !nCorrespondences )
{
// Nothing we can do !!
keepApproaching = false;
}
else
{
// Compute the estimated pose.
// (Method from paper of J.Gonzalez, Martinez y Morales)
// ----------------------------------------------------------------------
mrpt::math::TPose2D est_mean;
mrpt::tfest::se2_l2(correspondences, est_mean);
gaussPdf->mean = est_mean;
// If matching has not changed, decrease the thresholds:
// --------------------------------------------------------
keepApproaching = true;
if (!(fabs(lastMeanPose.x()-gaussPdf->mean.x())>options.minAbsStep_trans ||
fabs(lastMeanPose.y()-gaussPdf->mean.y())>options.minAbsStep_trans ||
fabs(math::wrapToPi(lastMeanPose.phi()-gaussPdf->mean.phi()))>options.minAbsStep_rot))
{
matchParams.maxDistForCorrespondence *= options.ALFA;
matchParams.maxAngularDistForCorrespondence *= options.ALFA;
if (matchParams.maxDistForCorrespondence < options.smallestThresholdDist )
keepApproaching = false;
if (++matchParams.offset_other_map_points>=options.corresponding_points_decimation)
matchParams.offset_other_map_points=0;
}
lastMeanPose = gaussPdf->mean;
} // end of "else, there are correspondences"
// Next iteration:
outInfo.nIterations++;
if (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist)
{
matchParams.maxDistForCorrespondence *= options.ALFA;
}
} while ( (keepApproaching && outInfo.nIterations<options.maxIterations) ||
(outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) );
// -------------------------------------------------
// Obtain the covariance matrix of the estimation
// -------------------------------------------------
if (!options.skip_cov_calculation && nCorrespondences)
{
switch(options.ICP_covariance_method)
{
// ----------------------------------------------
// METHOD: MSE linear estimation
// ----------------------------------------------
case icpCovLinealMSE:
mrpt::tfest::se2_l2(correspondences, *gaussPdf );
// Scale covariance:
gaussPdf->cov *= options.covariance_varPoints;
break;
// ----------------------------------------------
// METHOD: Method from Oxford MRG's "OXSMV2"
//
// It is the equivalent covariance resulting
// from a Levenberg-Maquardt optimization stage.
// ----------------------------------------------
case icpCovFiniteDifferences:
{
Eigen::Matrix<double,3,Eigen::Dynamic> D(3,nCorrespondences);
const TPose2D transf( gaussPdf->mean );
double ccos = cos(transf.phi);
double csin = sin(transf.phi);
double w1,w2,w3;
double q1,q2,q3;
double A,B;
double Axy = 0.01;
// Fill out D:
double rho2 = square( options.kernel_rho );
mrpt::utils::TMatchingPairList::iterator it;
size_t i;
for (i=0, it=correspondences.begin();i<nCorrespondences; ++i, ++it)
{
float other_x_trans = transf.x + ccos * it->other_x - csin * it->other_y;
float other_y_trans = transf.y + csin * it->other_x + ccos * it->other_y;
// Jacobian: dR2_dx
// --------------------------------------
w1= other_x_trans-Axy;
q1= kernel( square(it->this_x - w1)+ square( it->this_y - other_y_trans ), rho2 );
w2= other_x_trans;
q2= kernel( square(it->this_x - w2)+ square( it->this_y - other_y_trans ), rho2 );
w3= other_x_trans+Axy;
q3= kernel( square(it->this_x - w3)+ square( it->this_y - other_y_trans ), rho2 );
//interpolate
A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) );
B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2);
D(0,i) = (2*A*other_x_trans)+B;
// Jacobian: dR2_dy
// --------------------------------------
w1= other_y_trans-Axy;
q1= kernel( square(it->this_x - other_x_trans)+ square( it->this_y - w1 ), rho2 );
w2= other_y_trans;
q2= kernel( square(it->this_x - other_x_trans)+ square( it->this_y - w2 ), rho2 );
w3= other_y_trans+Axy;
q3= kernel( square(it->this_x - other_x_trans)+ square( it->this_y - w3 ), rho2 );
//interpolate
A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) );
B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2);
D(1,i) = (2*A*other_y_trans)+B;
// Jacobian: dR_dphi
// --------------------------------------
D(2,i) = D(0,i) * ( -csin * it->other_x - ccos * it->other_y ) +
D(1,i) * ( ccos * it->other_x - csin * it->other_y );
} // end for each corresp.
// COV = ( D*D^T + lamba*I )^-1
CMatrixDouble33 DDt = D*D.transpose();
for (i=0;i<3;i++) DDt( i,i ) += 1e-6; // Just to make sure the matrix is not singular, while not changing its covariance significantly.
DDt.inv(gaussPdf->cov);
}
break;
default:
THROW_EXCEPTION_FMT("Invalid value for ICP_covariance_method: %i", static_cast<int>(options.ICP_covariance_method));
}
}
outInfo.goodness = matchExtraResults.correspondencesRatio;
if (!nCorrespondences || options.skip_quality_calculation)
{
outInfo.quality = matchExtraResults.correspondencesRatio;
}
else
{
// Compute a crude estimate of the quality of scan matching at this local minimum:
// -----------------------------------------------------------------------------------
float Axy = matchParams.maxDistForCorrespondence*0.9f;
TPose2D P0( gaussPdf->mean );
TPose2D PX1(P0); PX1.x -= Axy;
TPose2D PX2(P0); PX2.x += Axy;
TPose2D PY1(P0); PY1.y -= Axy;
TPose2D PY2(P0); PY2.y += Axy;
matchParams.angularDistPivotPoint = TPoint3D( gaussPdf->mean.x(),gaussPdf->mean.y(),0);
m1->determineMatching2D(
m2, // The other map
P0, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float E0 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PX1, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EX1 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PX2, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EX2 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PY1, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EY1 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PY2, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EY2 = matchExtraResults.correspondencesRatio;
outInfo.quality= -max(EX1-E0, max(EX2-E0, max(EY1-E0 , EY2-E0 ) ) )/(E0+1e-1);
}
} // end of "if m2 is not empty"
// We'll return a CPosePDFGaussian or a CPosePDFSOG if RANSAC is enabled:
// -----------------------------------------------------------------------
// RANSAC?
if (options.doRANSAC)
{
mrpt::tfest::TSE2RobustParams params;
params.ransac_minSetSize = options.ransac_minSetSize;
params.ransac_maxSetSize = options.ransac_maxSetSize;
params.ransac_mahalanobisDistanceThreshold = options.ransac_mahalanobisDistanceThreshold;
params.ransac_nSimulations = options.ransac_nSimulations;
params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
params.ransac_algorithmForLandmarks = false;
mrpt::tfest::TSE2RobustResult results;
mrpt::tfest::se2_l2_robust(correspondences, options.normalizationStd, params, results);
SOG = CPosePDFSOG::Create();
*SOG = results.transformation;
// And return the SOG:
resultPDF = SOG;
}
else
{
// Return the gaussian distribution:
resultPDF = gaussPdf;
}
return resultPDF;
MRPT_END
}
