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