/*--------------------------------------------------------------- operator = ---------------------------------------------------------------*/ void CPose3DQuatPDFGaussian::copyFrom(const CPose3DQuatPDF &o) { if (this == &o) return; // It may be used sometimes // Convert to gaussian pdf: o.getCovarianceAndMean(cov,mean); }
/*--------------------------------------------------------------- inverse ---------------------------------------------------------------*/ void CPose3DQuatPDFGaussian::inverse(CPose3DQuatPDF &o) const { ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DQuatPDFGaussian)); CPose3DQuatPDFGaussian &out = static_cast<CPose3DQuatPDFGaussian&>(o); // COV: CMatrixFixedNumeric<double,3,7> df_dpose(UNINITIALIZED_MATRIX); double lx,ly,lz; mean.inverseComposePoint(0,0,0,lx,ly,lz, NULL, &df_dpose); CMatrixFixedNumeric<double,7,7> jacob; jacob.insertMatrix(0,0, df_dpose ); jacob.set_unsafe(3,3, 1); jacob.set_unsafe(4,4, -1); jacob.set_unsafe(5,5, -1); jacob.set_unsafe(6,6, -1); // C(0:2,0:2): H C H^t jacob.multiply_HCHt( this->cov, out.cov ); // Mean: out.mean.x(lx); out.mean.y(ly); out.mean.z(lz); this->mean.quat().conj( out.mean.quat() ); }
/*--------------------------------------------------------------- operator = ---------------------------------------------------------------*/ void CPose3DQuatPDFGaussianInf::copyFrom(const CPose3DQuatPDF &o) { if (this == &o) return; // It may be used sometimes // Convert to gaussian pdf: CMatrixDouble77 C(UNINITIALIZED_MATRIX); o.getCovarianceAndMean(C,this->mean); C.inv_fast(this->cov_inv); }