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