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