12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

normalization constant

release/4.3a0
Frank Dellaert 2022-12-30 13:58:54 -05:00
parent f22ada6c0a
commit ff51f4221e
2 changed files with 30 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/linear/GaussianConditional.cpp
View File

@ -168,26 +168,30 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
double GaussianConditional::logDeterminant() const { double GaussianConditional::logDeterminant() const {
double logDet; if (get_model()) {
if (this->get_model()) { Vector diag = R().diagonal();
Vector diag = this->R().diagonal(); get_model()->whitenInPlace(diag);
this->get_model()->whitenInPlace(diag); return diag.unaryExpr([](double x) { return log(x); }).sum();
logDet = diag.unaryExpr([](double x) { return log(x); }).sum();
} else { } else {
logDet = return R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
this->R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
} }
return logDet;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// density = exp(-error(x)) / sqrt((2*pi)^n*det(Sigma)) // normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
// log = -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) // log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
double GaussianConditional::logDensity(const VectorValues& x) const { double GaussianConditional::logNormalizationConstant() const {
constexpr double log2pi = 1.8378770664093454835606594728112; constexpr double log2pi = 1.8378770664093454835606594728112;
size_t n = d().size(); size_t n = d().size();
// log det(Sigma)) = - 2.0 * logDeterminant() // log det(Sigma)) = - 2.0 * logDeterminant()
return - error(x) - 0.5 * n * log2pi + logDeterminant(); return - 0.5 * n * log2pi + logDeterminant();
}
/* ************************************************************************* */
// density = k exp(-error(x))
// log = log(k) -error(x) - 0.5 * n*log(2*pi)
double GaussianConditional::logDensity(const VectorValues& x) const {
return logNormalizationConstant() - error(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */

15
gtsam/linear/GaussianConditional.h
View File

@ -169,7 +169,7 @@ namespace gtsam {
* *
* @return double * @return double
*/ */
double determinant() const { return exp(this->logDeterminant()); } inline double determinant() const { return exp(logDeterminant()); }
/** /**
* @brief Compute the log determinant of the R matrix. * @brief Compute the log determinant of the R matrix.
@ -184,6 +184,19 @@ namespace gtsam {
*/ */
double logDeterminant() const; double logDeterminant() const;
/**
* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
* log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
*/
double logNormalizationConstant() const;
/**
* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma))
*/
inline double normalizationConstant() const {
return exp(logNormalizationConstant());
}
/** /**
* Solves a conditional Gaussian and writes the solution into the entries of * Solves a conditional Gaussian and writes the solution into the entries of
* \c x for each frontal variable of the conditional. The parents are * \c x for each frontal variable of the conditional. The parents are