From 28b118ccda00988f5d9c9059ce17ca791ba42112 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 23 Dec 2022 11:09:35 +0530
Subject: [PATCH] GaussianConditional logDeterminant function to remove code
 duplication.

---
 gtsam/linear/GaussianBayesNet.cpp    |  9 +--------
 gtsam/linear/GaussianBayesTree-inl.h |  2 +-
 gtsam/linear/GaussianBayesTree.cpp   | 10 +---------
 gtsam/linear/GaussianConditional.cpp | 14 ++++++++++++++
 gtsam/linear/GaussianConditional.h   | 22 ++++++++++++++++++++++
 5 files changed, 39 insertions(+), 18 deletions(-)

diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp
index 6dcf662a9..41a734b34 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 d781533e6..e73420644 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<double,double>(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 a83475e26..b35252e72 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 951577641..60ddb1b7d 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 4822e3eaf..8af7f6602 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