/*--------------------------------------------------------------- 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 }