diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 6dcf662a90..41a734b34b 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -217,14 +217,7 @@ namespace gtsam { double GaussianBayesNet::logDeterminant() const { double logDet = 0.0; for (const sharedConditional& cg : *this) { - if (cg->get_model()) { - Vector diag = cg->R().diagonal(); - cg->get_model()->whitenInPlace(diag); - logDet += diag.unaryExpr([](double x) { return log(x); }).sum(); - } else { - logDet += - cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - } + logDet += cg->logDeterminant(); } return logDet; } diff --git a/gtsam/linear/GaussianBayesTree-inl.h b/gtsam/linear/GaussianBayesTree-inl.h index d781533e67..e734206442 100644 --- a/gtsam/linear/GaussianBayesTree-inl.h +++ b/gtsam/linear/GaussianBayesTree-inl.h @@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) { double result = 0.0; // this clique - result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun(log)).sum(); + result += clique->conditional()->logDeterminant(); // sum of children for(const typename BAYESTREE::sharedClique& child: clique->children_) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index a83475e26b..b35252e729 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -50,15 +50,7 @@ namespace gtsam { const GaussianBayesTreeClique::shared_ptr& clique, LogDeterminantData& parentSum) { auto cg = clique->conditional(); - double logDet; - if (cg->get_model()) { - Vector diag = cg->R().diagonal(); - cg->get_model()->whitenInPlace(diag); - logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); - } else { - logDet = - cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); - } + double logDet = cg->logDeterminant(); // Add the current clique's log-determinant to the overall sum (*parentSum.logDet) += logDet; return parentSum; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 9515776416..60ddb1b7d0 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -155,6 +155,20 @@ namespace gtsam { } } +/* ************************************************************************* */ +double GaussianConditional::logDeterminant() const { + double logDet; + if (this->get_model()) { + Vector diag = this->R().diagonal(); + this->get_model()->whitenInPlace(diag); + logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); + } else { + logDet = + this->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + } + return logDet; +} + /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 4822e3eaf7..8af7f66029 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -133,6 +133,28 @@ namespace gtsam { /** Get a view of the r.h.s. vector d */ const constBVector d() const { return BaseFactor::getb(); } + /** + * @brief Compute the log determinant of the Gaussian conditional. + * The determinant is computed using the R matrix, which is upper + * triangular. + * For numerical stability, the determinant is computed in log + * form, so it is a summation rather than a multiplication. + * + * @return double + */ + double logDeterminant() const; + + /** + * @brief Compute the determinant of the conditional from the + * upper-triangular R matrix. + * + * The determinant is computed in log form (hence summation) for numerical + * stability and then exponentiated. + * + * @return double + */ + double determinant() const { return exp(this->logDeterminant()); } + /** * Solves a conditional Gaussian and writes the solution into the entries of * \c x for each frontal variable of the conditional. The parents are