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 );
}
/*---------------------------------------------------------------
operator =
---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::copyFrom(const CPose3DPDF &o)
{
if (this == &o) return; // It may be used sometimes
if (IS_CLASS(&o, CPose3DPDFGaussianInf))
{ // It's my same class:
const CPose3DPDFGaussianInf *ptr = static_cast<const CPose3DPDFGaussianInf*>(&o);
mean = ptr->mean;
cov_inv = ptr->cov_inv;
}
else
{
// Convert to gaussian pdf:
CMatrixDouble66 cov(UNINITIALIZED_MATRIX);
o.getCovarianceAndMean(cov,mean);
cov.inv_fast(this->cov_inv);
}
}