/*--------------------------------------------------------------- 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 }
/*--------------------------------------------------------------- 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) ); } }
void CObservationStereoImagesFeatures::getDescriptionAsText(std::ostream &o) const { CObservation::getDescriptionAsText(o); o << "Homogeneous matrix for the sensor's 3D pose, relative to robot base:\n"; o << cameraPoseOnRobot.getHomogeneousMatrixVal() << cameraPoseOnRobot << endl; o << "Homogeneous matrix for the RIGHT camera's 3D pose, relative to LEFT camera reference system:\n"; o << rightCameraPose.getHomogeneousMatrixVal() << rightCameraPose << endl; o << "Intrinsic parameters matrix for the LEFT camera:"<< endl; CMatrixDouble33 aux = cameraLeft.intrinsicParams; o << aux.inMatlabFormat() << endl << aux << endl; o << "Distortion parameters vector for the LEFT camera:"<< endl << "[ "; for( unsigned int i = 0; i < 5; ++i ) o << cameraLeft.dist[i] << " "; o << "]" << endl; o << "Intrinsic parameters matrix for the RIGHT camera:"<< endl; aux = cameraRight.intrinsicParams; o << aux.inMatlabFormat() << endl << aux << endl; o << "Distortion parameters vector for the RIGHT camera:"<< endl << "[ "; for( unsigned int i = 0; i < 5; ++i ) o << cameraRight.dist[i] << " "; o << "]"<< endl; o << endl << format(" Image size: %ux%u pixels\n", (unsigned int)cameraLeft.ncols, (unsigned int)cameraLeft.nrows ); o << endl << format(" Number of features in images: %u\n", (unsigned int)theFeatures.size() ); }
/*--------------------------------------------------------------- getEstimatedCovariance ---------------------------------------------------------------*/ void CPosePDFSOG::getCovarianceAndMean(CMatrixDouble33 &estCov, CPose2D &estMean2D) const { size_t N = m_modes.size(); this->getMean(estMean2D); estCov.zeros(); if (N) { // 1) Get the mean: double sumW = 0; CMatrixDouble31 estMeanMat = CMatrixDouble31(estMean2D); CMatrixDouble33 temp; CMatrixDouble31 estMean_i; for (const_iterator it=m_modes.begin();it!=m_modes.end();++it) { double w; sumW += w = exp((it)->log_w); estMean_i = CMatrixDouble31( (it)->mean ); estMean_i -= estMeanMat; temp.multiply_AAt(estMean_i); temp+= (it)->cov; temp*=w; estCov += temp; } if (sumW!=0) estCov *= (1.0/sumW); } }
/*--------------------------------------------------------------- getCovarianceAndMean ---------------------------------------------------------------*/ void CPointPDFSOG::getCovarianceAndMean(CMatrixDouble33 &estCov, CPoint3D &p) const { size_t N = m_modes.size(); getMean(p); estCov.zeros(); if (N) { // 1) Get the mean: double sumW = 0; CMatrixDouble31 estMean = CMatrixDouble31(p); CListGaussianModes::const_iterator it; CMatrixDouble33 partCov; for (it=m_modes.begin();it!=m_modes.end();++it) { double w; sumW += w = exp(it->log_w); // estCov += w * ( it->val.cov + ((estMean_i-estMean)*(~(estMean_i-estMean))) ); CMatrixDouble31 estMean_i = CMatrixDouble31(it->val.mean); estMean_i -=estMean; partCov.multiply_AAt(estMean_i); partCov+=it->val.cov; partCov*=w; estCov += partCov; } if (sumW!=0) estCov *= (1.0/sumW); } }
static CPosePDFGaussian generateRandomPose2DPDF(double x,double y, double phi, double std_scale) { CMatrixDouble31 r; mrpt::random::randomGenerator.drawGaussian1DMatrix(r, 0,std_scale); CMatrixDouble33 cov; cov.multiply_AAt(r); // random semi-definite positive matrix: for (int i=0;i<3;i++) cov(i,i)+=1e-7; CPosePDFGaussian pdf( CPose2D(x,y,phi), cov ); return pdf; }
/*--------------------------------------------------------------- setPosePDF ---------------------------------------------------------------*/ void CPoseRandomSampler::setPosePDF(const CPosePDF* pdf) { MRPT_START clear(); m_pdf2D.reset(dynamic_cast<CPosePDF*>(pdf->clone())); // According to the PDF type: if (IS_CLASS(m_pdf2D.get(), CPosePDFGaussian)) { const CPosePDFGaussian* gPdf = static_cast<const CPosePDFGaussian*>(pdf); const CMatrixDouble33& cov = gPdf->cov; m_fastdraw_gauss_M_2D = gPdf->mean; /** Computes the eigenvalues/eigenvector decomposition of this matrix, * so that: M = Z · D · Z<sup>T</sup>, where columns in Z are the * eigenvectors and the diagonal matrix D contains the eigenvalues * as diagonal elements, sorted in <i>ascending</i> order. */ CMatrixDouble33 D; cov.eigenVectors(m_fastdraw_gauss_Z3, D); // Scale eigenvectors with eigenvalues: D = D.array().sqrt().matrix(); m_fastdraw_gauss_Z3.multiply(m_fastdraw_gauss_Z3, D); } else if (IS_CLASS(m_pdf2D.get(), CPosePDFParticles)) { return; // Nothing to prepare. } else { THROW_EXCEPTION_FMT( "Unsupported class: %s", m_pdf2D->GetRuntimeClass()->className); } MRPT_END }
/*--------------------------------------------------------------- getEstimatedCovariance ---------------------------------------------------------------*/ void CPosePDFParticles::getCovarianceAndMean(CMatrixDouble33 &cov, CPose2D &mean) const { cov.zeros(); getMean(mean); size_t i,n = m_particles.size(); double var_x=0,var_y=0,var_p=0,var_xy=0,var_xp=0,var_yp=0; double mean_phi = mean.phi(); if (mean_phi<0) mean_phi = M_2PI + mean_phi; double lin_w_sum = 0; for (i=0;i<n;i++) lin_w_sum+= exp( m_particles[i].log_w ); if (lin_w_sum==0) lin_w_sum=1; for (i=0;i<n;i++) { double w = exp( m_particles[i].log_w ) / lin_w_sum; // Manage 1 PI range: double err_x = m_particles[i].d->x() - mean.x(); double err_y = m_particles[i].d->y() - mean.y(); double err_phi = math::wrapToPi( fabs(m_particles[i].d->phi() - mean_phi) ); var_x+= square(err_x)*w; var_y+= square(err_y)*w; var_p+= square(err_phi)*w; var_xy+= err_x*err_y*w; var_xp+= err_x*err_phi*w; var_yp+= err_y*err_phi*w; } if (n<2) { // Not enought information to estimate the variance: } else { // Unbiased estimation of variance: cov(0,0) = var_x; cov(1,1) = var_y; cov(2,2) = var_p; cov(1,0) = cov(0,1) = var_xy; cov(2,0) = cov(0,2) = var_xp; cov(1,2) = cov(2,1) = var_yp; } }
/*--------------------------------------------------------------- jacobiansPoseComposition ---------------------------------------------------------------*/ void CPosePDF::jacobiansPoseComposition( const CPose2D &x, const CPose2D &u, CMatrixDouble33 &df_dx, CMatrixDouble33 &df_du, const bool compute_df_dx, const bool compute_df_du ) { const double spx = sin(x.phi()); const double cpx = cos(x.phi()); if (compute_df_dx) { /* df_dx = [ 1, 0, -sin(phi_x)*x_u-cos(phi_x)*y_u ] [ 0, 1, cos(phi_x)*x_u-sin(phi_x)*y_u ] [ 0, 0, 1 ] */ df_dx.unit(3,1.0); const double xu = u.x(); const double yu = u.y(); df_dx.get_unsafe(0,2) = -spx*xu-cpx*yu; df_dx.get_unsafe(1,2) = cpx*xu-spx*yu; } if (compute_df_du) { /* df_du = [ cos(phi_x) , -sin(phi_x) , 0 ] [ sin(phi_x) , cos(phi_x) , 0 ] [ 0 , 0 , 1 ] */ // This is the homogeneous matrix of "x": df_du.get_unsafe(0,2) = df_du.get_unsafe(1,2) = df_du.get_unsafe(2,0) = df_du.get_unsafe(2,1) = 0; df_du.get_unsafe(2,2) = 1; df_du.get_unsafe(0,0) = cpx; df_du.get_unsafe(0,1) = -spx; df_du.get_unsafe(1,0) = spx; df_du.get_unsafe(1,1) = cpx; } }
/*---------------------------------------------------------------------------- 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 }
/*--------------------------------------------------------------- HornMethod ---------------------------------------------------------------*/ double scanmatching::HornMethod( const vector_double &inVector, vector_double &outVector, // The output vector bool forceScaleToUnity ) { MRPT_START vector_double input; input.resize( inVector.size() ); input = inVector; outVector.resize( 7 ); // Compute the centroids TPoint3D cL(0,0,0), cR(0,0,0); const size_t nMatches = input.size()/6; ASSERT_EQUAL_(input.size()%6, 0) for( unsigned int i = 0; i < nMatches; i++ ) { cL.x += input[i*6+3]; cL.y += input[i*6+4]; cL.z += input[i*6+5]; cR.x += input[i*6+0]; cR.y += input[i*6+1]; cR.z += input[i*6+2]; } ASSERT_ABOVE_(nMatches,0) const double F = 1.0/nMatches; cL *= F; cR *= F; CMatrixDouble33 S; // S.zeros(); // Zeroed by default // Substract the centroid and compute the S matrix of cross products for( unsigned int i = 0; i < nMatches; i++ ) { input[i*6+3] -= cL.x; input[i*6+4] -= cL.y; input[i*6+5] -= cL.z; input[i*6+0] -= cR.x; input[i*6+1] -= cR.y; input[i*6+2] -= cR.z; S.get_unsafe(0,0) += input[i*6+3]*input[i*6+0]; S.get_unsafe(0,1) += input[i*6+3]*input[i*6+1]; S.get_unsafe(0,2) += input[i*6+3]*input[i*6+2]; S.get_unsafe(1,0) += input[i*6+4]*input[i*6+0]; S.get_unsafe(1,1) += input[i*6+4]*input[i*6+1]; S.get_unsafe(1,2) += input[i*6+4]*input[i*6+2]; S.get_unsafe(2,0) += input[i*6+5]*input[i*6+0]; S.get_unsafe(2,1) += input[i*6+5]*input[i*6+1]; S.get_unsafe(2,2) += input[i*6+5]*input[i*6+2]; } // Construct the N matrix CMatrixDouble44 N; // N.zeros(); // Zeroed by default N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) ); N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) ); N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) ); N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) ); N.set_unsafe( 1,0,N.get_unsafe(0,1) ); N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) ); N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) ); N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) ); N.set_unsafe( 2,0,N.get_unsafe(0,2) ); N.set_unsafe( 2,1,N.get_unsafe(1,2) ); N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) ); N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) ); N.set_unsafe( 3,0,N.get_unsafe(0,3) ); N.set_unsafe( 3,1,N.get_unsafe(1,3) ); N.set_unsafe( 3,2,N.get_unsafe(2,3) ); N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) ); // q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z) CMatrixDouble44 Z, D; vector_double v; N.eigenVectors( Z, D ); Z.extractCol( Z.getColCount()-1, v ); ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 ); // Make q_r > 0 if( v[0] < 0 ){ v[0] *= -1; v[1] *= -1; v[2] *= -1; v[3] *= -1; } CPose3DQuat q; // Create a pose rotation with the quaternion for(unsigned int i = 0; i < 4; i++ ) // insert the quaternion part outVector[i+3] = q[i+3] = v[i]; // Compute scale double num = 0.0; double den = 0.0; for( unsigned int i = 0; i < nMatches; i++ ) { num += square( input[i*6+0] ) + square( input[i*6+1] ) + square( input[i*6+2] ); den += square( input[i*6+3] ) + square( input[i*6+4] ) + square( input[i*6+5] ); } // end-for // The scale: double s = std::sqrt( num/den ); // Enforce scale to be 1 if( forceScaleToUnity ) s = 1.0; TPoint3D pp; q.composePoint( cL.x, cL.y, cL.z, pp.x, pp.y, pp.z ); pp*=s; outVector[0] = cR.x - pp.x; // X outVector[1] = cR.y - pp.y; // Y outVector[2] = cR.z - pp.z; // Z return s; // return scale MRPT_END }
/*--------------------------------------------------------------- se3_l2 (old "HornMethod()") ---------------------------------------------------------------*/ bool se3_l2_internal( std::vector<mrpt::math::TPoint3D> & points_this, // IN/OUT: It gets modified! std::vector<mrpt::math::TPoint3D> & points_other, // IN/OUT: It gets modified! mrpt::poses::CPose3DQuat & out_transform, double & out_scale, bool forceScaleToUnity) { MRPT_START ASSERT_EQUAL_(points_this.size(),points_other.size()) // Compute the centroids TPoint3D ct_others(0,0,0), ct_this(0,0,0); const size_t nMatches = points_this.size(); if (nMatches<3) return false; // Nothing we can estimate without 3 points!! for(size_t i = 0; i < nMatches; i++ ) { ct_others += points_other[i]; ct_this += points_this [i]; } const double F = 1.0/nMatches; ct_others *= F; ct_this *= F; CMatrixDouble33 S; // Zeroed by default // Substract the centroid and compute the S matrix of cross products for(size_t i = 0; i < nMatches; i++ ) { points_this[i] -= ct_this; points_other[i] -= ct_others; S.get_unsafe(0,0) += points_other[i].x * points_this[i].x; S.get_unsafe(0,1) += points_other[i].x * points_this[i].y; S.get_unsafe(0,2) += points_other[i].x * points_this[i].z; S.get_unsafe(1,0) += points_other[i].y * points_this[i].x; S.get_unsafe(1,1) += points_other[i].y * points_this[i].y; S.get_unsafe(1,2) += points_other[i].y * points_this[i].z; S.get_unsafe(2,0) += points_other[i].z * points_this[i].x; S.get_unsafe(2,1) += points_other[i].z * points_this[i].y; S.get_unsafe(2,2) += points_other[i].z * points_this[i].z; } // Construct the N matrix CMatrixDouble44 N; // Zeroed by default N.set_unsafe( 0,0,S.get_unsafe(0,0) + S.get_unsafe(1,1) + S.get_unsafe(2,2) ); N.set_unsafe( 0,1,S.get_unsafe(1,2) - S.get_unsafe(2,1) ); N.set_unsafe( 0,2,S.get_unsafe(2,0) - S.get_unsafe(0,2) ); N.set_unsafe( 0,3,S.get_unsafe(0,1) - S.get_unsafe(1,0) ); N.set_unsafe( 1,0,N.get_unsafe(0,1) ); N.set_unsafe( 1,1,S.get_unsafe(0,0) - S.get_unsafe(1,1) - S.get_unsafe(2,2) ); N.set_unsafe( 1,2,S.get_unsafe(0,1) + S.get_unsafe(1,0) ); N.set_unsafe( 1,3,S.get_unsafe(2,0) + S.get_unsafe(0,2) ); N.set_unsafe( 2,0,N.get_unsafe(0,2) ); N.set_unsafe( 2,1,N.get_unsafe(1,2) ); N.set_unsafe( 2,2,-S.get_unsafe(0,0) + S.get_unsafe(1,1) - S.get_unsafe(2,2) ); N.set_unsafe( 2,3,S.get_unsafe(1,2) + S.get_unsafe(2,1) ); N.set_unsafe( 3,0,N.get_unsafe(0,3) ); N.set_unsafe( 3,1,N.get_unsafe(1,3) ); N.set_unsafe( 3,2,N.get_unsafe(2,3) ); N.set_unsafe( 3,3,-S.get_unsafe(0,0) - S.get_unsafe(1,1) + S.get_unsafe(2,2) ); // q is the quaternion correspondent to the greatest eigenvector of the N matrix (last column in Z) CMatrixDouble44 Z, D; vector<double> v; N.eigenVectors( Z, D ); Z.extractCol( Z.getColCount()-1, v ); ASSERTDEB_( fabs( sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3] ) - 1.0 ) < 0.1 ); // Make q_r > 0 if( v[0] < 0 ) { v[0] *= -1; v[1] *= -1; v[2] *= -1; v[3] *= -1; } // out_transform: Create a pose rotation with the quaternion for(unsigned int i = 0; i < 4; i++ ) // insert the quaternion part out_transform[i+3] = v[i]; // Compute scale double s; if( forceScaleToUnity ) { s = 1.0; // Enforce scale to be 1 } else { double num = 0.0; double den = 0.0; for(size_t i = 0; i < nMatches; i++ ) { num += square( points_other[i].x ) + square( points_other[i].y ) + square( points_other[i].z ); den += square( points_this[i].x ) + square( points_this[i].y ) + square( points_this[i].z ); } // end-for // The scale: s = std::sqrt( num/den ); } TPoint3D pp; out_transform.composePoint( ct_others.x, ct_others.y, ct_others.z, pp.x, pp.y, pp.z ); pp*=s; out_transform[0] = ct_this.x - pp.x; // X out_transform[1] = ct_this.y - pp.y; // Y out_transform[2] = ct_this.z - pp.z; // Z out_scale=s; // return scale return true; MRPT_END } // end se3_l2_internal()
/*--------------------------------------------------------------- leastSquareErrorRigidTransformation Compute the best transformation (x,y,phi) given a set of correspondences between 2D points in two different maps. This method is intensively used within ICP. ---------------------------------------------------------------*/ bool tfest::se2_l2( const TMatchingPairList& in_correspondences, TPose2D& out_transformation, CMatrixDouble33* out_estimateCovariance) { MRPT_START const size_t N = in_correspondences.size(); if (N < 2) return false; const float N_inv = 1.0f / N; // For efficiency, keep this value. // ---------------------------------------------------------------------- // Compute the estimated pose. Notation from the paper: // "Mobile robot motion estimation by 2d scan matching with genetic and // iterative // closest point algorithms", J.L. Martinez Rodriguez, A.J. Gonzalez, J. Morales // Rodriguez, A. Mandow Andaluz, A. J. Garcia Cerezo, // Journal of Field Robotics, 2006. // ---------------------------------------------------------------------- // ---------------------------------------------------------------------- // For the formulas of the covariance, see: // http://www.mrpt.org/Paper:Occupancy_Grid_Matching // and Jose Luis Blanco's PhD thesis. // ---------------------------------------------------------------------- #if MRPT_HAS_SSE2 // SSE vectorized version: //{ // TMatchingPair dumm; // MRPT_COMPILE_TIME_ASSERT(sizeof(dumm.this_x)==sizeof(float)) // MRPT_COMPILE_TIME_ASSERT(sizeof(dumm.other_x)==sizeof(float)) //} __m128 sum_a_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) __m128 sum_b_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) // [ f0 f1 f2 f3 ] // xa*xb ya*yb xa*yb xb*ya __m128 sum_ab_xyz = _mm_setzero_ps(); // All 4 zeros (0.0f) for (TMatchingPairList::const_iterator corrIt = in_correspondences.begin(); corrIt != in_correspondences.end(); corrIt++) { // Get the pair of points in the correspondence: // a_xyyx = [ xa ay | xa ya ] // b_xyyx = [ xb yb | yb xb ] // (product) // [ xa*xb ya*yb xa*yb xb*ya // LO0 LO1 HI2 HI3 // Note: _MM_SHUFFLE(hi3,hi2,lo1,lo0) const __m128 a_xyz = _mm_loadu_ps(&corrIt->this_x); // *Unaligned* load const __m128 b_xyz = _mm_loadu_ps(&corrIt->other_x); // *Unaligned* load const __m128 a_xyxy = _mm_shuffle_ps(a_xyz, a_xyz, _MM_SHUFFLE(1, 0, 1, 0)); const __m128 b_xyyx = _mm_shuffle_ps(b_xyz, b_xyz, _MM_SHUFFLE(0, 1, 1, 0)); // Compute the terms: sum_a_xyz = _mm_add_ps(sum_a_xyz, a_xyz); sum_b_xyz = _mm_add_ps(sum_b_xyz, b_xyz); // [ f0 f1 f2 f3 ] // xa*xb ya*yb xa*yb xb*ya sum_ab_xyz = _mm_add_ps(sum_ab_xyz, _mm_mul_ps(a_xyxy, b_xyyx)); } alignas(MRPT_MAX_ALIGN_BYTES) float sums_a[4], sums_b[4]; _mm_store_ps(sums_a, sum_a_xyz); _mm_store_ps(sums_b, sum_b_xyz); const float& SumXa = sums_a[0]; const float& SumYa = sums_a[1]; const float& SumXb = sums_b[0]; const float& SumYb = sums_b[1]; // Compute all four means: const __m128 Ninv_4val = _mm_set1_ps(N_inv); // load 4 copies of the same value sum_a_xyz = _mm_mul_ps(sum_a_xyz, Ninv_4val); sum_b_xyz = _mm_mul_ps(sum_b_xyz, Ninv_4val); // means_a[0]: mean_x_a // means_a[1]: mean_y_a // means_b[0]: mean_x_b // means_b[1]: mean_y_b alignas(MRPT_MAX_ALIGN_BYTES) float means_a[4], means_b[4]; _mm_store_ps(means_a, sum_a_xyz); _mm_store_ps(means_b, sum_b_xyz); const float& mean_x_a = means_a[0]; const float& mean_y_a = means_a[1]; const float& mean_x_b = means_b[0]; const float& mean_y_b = means_b[1]; // Sxx Syy Sxy Syx // xa*xb ya*yb xa*yb xb*ya alignas(MRPT_MAX_ALIGN_BYTES) float cross_sums[4]; _mm_store_ps(cross_sums, sum_ab_xyz); const float& Sxx = cross_sums[0]; const float& Syy = cross_sums[1]; const float& Sxy = cross_sums[2]; const float& Syx = cross_sums[3]; // Auxiliary variables Ax,Ay: const float Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb; const float Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa; #else // Non vectorized version: float SumXa = 0, SumXb = 0, SumYa = 0, SumYb = 0; float Sxx = 0, Sxy = 0, Syx = 0, Syy = 0; for (TMatchingPairList::const_iterator corrIt = in_correspondences.begin(); corrIt != in_correspondences.end(); corrIt++) { // Get the pair of points in the correspondence: const float xa = corrIt->this_x; const float ya = corrIt->this_y; const float xb = corrIt->other_x; const float yb = corrIt->other_y; // Compute the terms: SumXa += xa; SumYa += ya; SumXb += xb; SumYb += yb; Sxx += xa * xb; Sxy += xa * yb; Syx += ya * xb; Syy += ya * yb; } // End of "for all correspondences"... const float mean_x_a = SumXa * N_inv; const float mean_y_a = SumYa * N_inv; const float mean_x_b = SumXb * N_inv; const float mean_y_b = SumYb * N_inv; // Auxiliary variables Ax,Ay: const float Ax = N * (Sxx + Syy) - SumXa * SumXb - SumYa * SumYb; const float Ay = SumXa * SumYb + N * (Syx - Sxy) - SumXb * SumYa; #endif out_transformation.phi = (Ax != 0 || Ay != 0) ? atan2(static_cast<double>(Ay), static_cast<double>(Ax)) : 0.0; const double ccos = cos(out_transformation.phi); const double csin = sin(out_transformation.phi); out_transformation.x = mean_x_a - mean_x_b * ccos + mean_y_b * csin; out_transformation.y = mean_y_a - mean_x_b * csin - mean_y_b * ccos; if (out_estimateCovariance) { CMatrixDouble33* C = out_estimateCovariance; // less typing! // Compute the normalized covariance matrix: // ------------------------------------------- double var_x_a = 0, var_y_a = 0, var_x_b = 0, var_y_b = 0; const double N_1_inv = 1.0 / (N - 1); // 0) Precompute the unbiased variances estimations: // ---------------------------------------------------- for (TMatchingPairList::const_iterator corrIt = in_correspondences.begin(); corrIt != in_correspondences.end(); corrIt++) { var_x_a += square(corrIt->this_x - mean_x_a); var_y_a += square(corrIt->this_y - mean_y_a); var_x_b += square(corrIt->other_x - mean_x_b); var_y_b += square(corrIt->other_y - mean_y_b); } var_x_a *= N_1_inv; // /= (N-1) var_y_a *= N_1_inv; var_x_b *= N_1_inv; var_y_b *= N_1_inv; // 1) Compute BETA = s_Delta^2 / s_p^2 // -------------------------------- const double BETA = (var_x_a + var_y_a + var_x_b + var_y_b) * pow(static_cast<double>(N), 2.0) * static_cast<double>(N - 1); // 2) And the final covariance matrix: // (remember: this matrix has yet to be // multiplied by var_p to be the actual covariance!) // ------------------------------------------------------- const double D = square(Ax) + square(Ay); C->get_unsafe(0, 0) = 2.0 * N_inv + BETA * square((mean_x_b * Ay + mean_y_b * Ax) / D); C->get_unsafe(1, 1) = 2.0 * N_inv + BETA * square((mean_x_b * Ax - mean_y_b * Ay) / D); C->get_unsafe(2, 2) = BETA / D; C->get_unsafe(0, 1) = C->get_unsafe(1, 0) = -BETA * (mean_x_b * Ay + mean_y_b * Ax) * (mean_x_b * Ax - mean_y_b * Ay) / square(D); C->get_unsafe(0, 2) = C->get_unsafe(2, 0) = BETA * (mean_x_b * Ay + mean_y_b * Ax) / pow(D, 1.5); C->get_unsafe(1, 2) = C->get_unsafe(2, 1) = BETA * (mean_y_b * Ay - mean_x_b * Ax) / pow(D, 1.5); } return true; MRPT_END }