void CEllipsoid_setFromPosePDF(CEllipsoid& self, CPose3DPDF& posePDF)
{
CPose3D meanPose;
CMatrixDouble66 COV;
posePDF.getCovarianceAndMean(COV, meanPose);
CMatrixDouble33 COV3 = COV.block(0,0,3,3);
self.setLocation(meanPose.x(), meanPose.y(), meanPose.z() + 0.001 );
self.setCovMatrix(COV3, COV3(2,2)==0 ? 2:3 );
}
static CPose3DPDFGaussian generateRandomPose3DPDF(double x,double y, double z, double yaw, double pitch, double roll, double std_scale)
{
CMatrixDouble61 r;
mrpt::random::randomGenerator.drawGaussian1DMatrix(r, 0,std_scale);
CMatrixDouble66 cov;
cov.multiply_AAt(r); // random semi-definite positive matrix:
for (int i=0;i<6;i++) cov(i,i)+=1e-7;
CPose3DPDFGaussian p6pdf( CPose3D(x,y,z,yaw,pitch,roll), cov );
return p6pdf;
}
/*---------------------------------------------------------------
copyFrom 3D
---------------------------------------------------------------*/
void CPosePDFGaussian::copyFrom(const CPose3DPDF &o)
{
// Convert to gaussian pdf:
mean = CPose2D(o.getMeanVal());
CMatrixDouble66 C;
o.getCovariance(C);
// Clip to 3x3:
C(2,0)=C(0,2) = C(0,3);
C(2,1)=C(1,2) = C(1,3);
C(2,2)= C(3,3);
cov = C.block(0,0,3,3);
}
/*---------------------------------------------------------------
setPosePDF
---------------------------------------------------------------*/
void CPoseRandomSampler::setPosePDF(const CPose3DPDF* pdf)
{
MRPT_START
clear();
m_pdf3D.reset(dynamic_cast<CPose3DPDF*>(pdf->clone()));
// According to the PDF type:
if (IS_CLASS(m_pdf3D.get(), CPose3DPDFGaussian))
{
const CPose3DPDFGaussian* gPdf =
static_cast<const CPose3DPDFGaussian*>(pdf);
const CMatrixDouble66& cov = gPdf->cov;
m_fastdraw_gauss_M_3D = 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.
*/
CMatrixDouble66 D;
cov.eigenVectors(m_fastdraw_gauss_Z6, D);
// Scale eigenvectors with eigenvalues:
D = D.array().sqrt().matrix();
m_fastdraw_gauss_Z6.multiply(m_fastdraw_gauss_Z6, D);
}
else if (IS_CLASS(m_pdf3D.get(), CPose3DPDFParticles))
{
return; // Nothing to prepare.
}
else
{
THROW_EXCEPTION_FMT(
"Unsoported class: %s", m_pdf3D->GetRuntimeClass()->className);
}
MRPT_END
}
/*---------------------------------------------------------------
getCovarianceAndMean
---------------------------------------------------------------*/
void CPose3DPDFParticles::getCovarianceAndMean(
CMatrixDouble66& cov, CPose3D& mean) const
{
MRPT_START
getMean(mean); // First! the mean value:
// Now the covariance:
cov.zeros();
CVectorDouble vars;
vars.assign(6, 0.0); // The diagonal of the final covariance matrix
// Elements off the diagonal of the covariance matrix:
double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
double std_zya = 0, std_zp = 0, std_zr = 0;
double std_yap = 0, std_yar = 0;
double std_pr = 0;
// Mean values in [0, 2pi] range:
double mean_yaw = mean.yaw();
double mean_pitch = mean.pitch();
double mean_roll = mean.roll();
if (mean_yaw < 0) mean_yaw += M_2PI;
if (mean_pitch < 0) mean_pitch += M_2PI;
if (mean_roll < 0) mean_roll += M_2PI;
// Enought information to estimate the covariance?
if (m_particles.size() < 2) return;
// Sum all weight values:
double W = 0;
for (CPose3DPDFParticles::CParticleList::const_iterator it =
m_particles.begin();
it != m_particles.end(); ++it)
W += exp(it->log_w);
ASSERT_(W > 0);
// Compute covariance:
for (CPose3DPDFParticles::CParticleList::const_iterator it =
m_particles.begin();
it != m_particles.end(); ++it)
{
double w = exp(it->log_w) / W;
// Manage 1 PI range:
double err_yaw = wrapToPi(fabs(it->d->yaw() - mean_yaw));
double err_pitch = wrapToPi(fabs(it->d->pitch() - mean_pitch));
double err_roll = wrapToPi(fabs(it->d->roll() - mean_roll));
double err_x = it->d->x() - mean.x();
double err_y = it->d->y() - mean.y();
double err_z = it->d->z() - mean.z();
vars[0] += square(err_x) * w;
vars[1] += square(err_y) * w;
vars[2] += square(err_z) * w;
vars[3] += square(err_yaw) * w;
vars[4] += square(err_pitch) * w;
vars[5] += square(err_roll) * w;
std_xy += err_x * err_y * w;
std_xz += err_x * err_z * w;
std_xya += err_x * err_yaw * w;
std_xp += err_x * err_pitch * w;
std_xr += err_x * err_roll * w;
std_yz += err_y * err_z * w;
std_yya += err_y * err_yaw * w;
std_yp += err_y * err_pitch * w;
std_yr += err_y * err_roll * w;
std_zya += err_z * err_yaw * w;
std_zp += err_z * err_pitch * w;
std_zr += err_z * err_roll * w;
std_yap += err_yaw * err_pitch * w;
std_yar += err_yaw * err_roll * w;
std_pr += err_pitch * err_roll * w;
} // end for it
// Unbiased estimation of variance:
cov(0, 0) = vars[0];
cov(1, 1) = vars[1];
cov(2, 2) = vars[2];
cov(3, 3) = vars[3];
cov(4, 4) = vars[4];
cov(5, 5) = vars[5];
cov(1, 0) = cov(0, 1) = std_xy;
cov(2, 0) = cov(0, 2) = std_xz;
cov(3, 0) = cov(0, 3) = std_xya;
cov(4, 0) = cov(0, 4) = std_xp;
cov(5, 0) = cov(0, 5) = std_xr;
cov(2, 1) = cov(1, 2) = std_yz;
cov(3, 1) = cov(1, 3) = std_yya;
cov(4, 1) = cov(1, 4) = std_yp;
cov(5, 1) = cov(1, 5) = std_yr;
cov(3, 2) = cov(2, 3) = std_zya;
cov(4, 2) = cov(2, 4) = std_zp;
cov(5, 2) = cov(2, 5) = std_zr;
cov(4, 3) = cov(3, 4) = std_yap;
cov(5, 3) = cov(3, 5) = std_yar;
cov(5, 4) = cov(4, 5) = std_pr;
MRPT_END
}