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