/*---------------------------------------------------------------
productIntegralWith
---------------------------------------------------------------*/
double CPointPDFGaussian::productIntegralWith( const CPointPDFGaussian &p) const
{
MRPT_START
// --------------------------------------------------------------
// 12/APR/2009 - Jose Luis Blanco:
// The integral over all the variable space of the product of two
// Gaussians variables amounts to simply the evaluation of
// a normal PDF at (0,0), with mean=M1-M2 and COV=COV1+COV2
// ---------------------------------------------------------------
CMatrixDouble33 C = cov; C+=p.cov; // Sum of covs:
CMatrixDouble33 C_inv;
C.inv(C_inv);
CMatrixDouble31 MU(UNINITIALIZED_MATRIX); // Diff. of means
MU.get_unsafe(0,0) = mean.x() - p.mean.x();
MU.get_unsafe(1,0) = mean.y() - p.mean.y();
MU.get_unsafe(2,0) = mean.z() - p.mean.z();
return std::pow( M_2PI, -0.5*state_length )
* (1.0/std::sqrt( C.det() ))
* exp( -0.5* MU.multiply_HtCH_scalar(C_inv) );
MRPT_END
}
