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