From 364b4b4a755cdb1c5299dc770cbade9357d24245 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 20 Sep 2024 04:35:29 -0400
Subject: [PATCH] logDetR method which leverages noise model for efficiency.
 Build logDeterminant and logNormalizationConstant on top of it.

---
 gtsam/linear/NoiseModel.cpp | 37 +++++++++++++++++++++++++++++--------
 gtsam/linear/NoiseModel.h   | 28 +++++++++++++++++++---------
 2 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index af3081b9f..de69fdce9 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -239,21 +239,31 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
 Matrix Gaussian::information() const { return R().transpose() * R(); }
 
 /* *******************************************************************************/
-double Gaussian::logNormalizationConstant() const {
+double Gaussian::logDetR() const {
+  double logDetR =
+      R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
+  return logDetR;
+}
+
+/* *******************************************************************************/
+double Gaussian::logDeterminant() const {
   // Since noise models are Gaussian, we can get the logDeterminant easily
   // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1}
   // log det(Sigma) = -log(det(R'R)) = -2*log(det(R))
-  // Hence, log det(Sigma)) = -2.0 * logDeterminant()
-  // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant())
-  //     = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant())
-  //     = -0.5*n*log(2*pi) + logDeterminant()
-  double logDetR =
-      R().diagonal().unaryExpr([](double x) { return log(x); }).sum();
+  // Hence, log det(Sigma)) = -2.0 * logDetR()
+  return -2.0 * logDetR();
+}
 
+/* *******************************************************************************/
+double Gaussian::logNormalizationConstant() const {
+  // log(det(Sigma)) = -2.0 * logDetR
+  // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDetR())
+  //     = -0.5*n*log(2*pi) + (0.5*2.0 * logDetR())
+  //     = -0.5*n*log(2*pi) + logDetR()
   size_t n = dim();
   constexpr double log2pi = 1.8378770664093454835606594728112;
   // Get 1/log(\sqrt(|2pi Sigma|)) = -0.5*log(|2pi Sigma|)
-  return -0.5 * n * log2pi + logDetR;
+  return -0.5 * n * log2pi + logDetR();
 }
 
 
@@ -333,6 +343,11 @@ void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
   H = invsigmas().asDiagonal() * H;
 }
 
+/* *******************************************************************************/
+double Diagonal::logDetR() const {
+  return invsigmas_.unaryExpr([](double x) { return log(x); }).sum();
+}
+
 /* ************************************************************************* */
 // Constrained
 /* ************************************************************************* */
@@ -661,6 +676,9 @@ void Isotropic::WhitenInPlace(Eigen::Block<Matrix> H) const {
   H *= invsigma_;
 }
 
+/* *******************************************************************************/
+double Isotropic::logDetR() const { return log(invsigma_) * dim(); }
+
 /* ************************************************************************* */
 // Unit
 /* ************************************************************************* */
@@ -673,6 +691,9 @@ double Unit::squaredMahalanobisDistance(const Vector& v) const {
   return v.dot(v);
 }
 
+/* *******************************************************************************/
+double Unit::logDetR() const { return 0.0; }
+
 /* ************************************************************************* */
 // Robust
 /* ************************************************************************* */
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index b8e6c09bc..91c11d410 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -183,6 +183,8 @@ namespace gtsam {
         return *sqrt_information_;
       }
 
+      /// Compute the log of |R|. Used for computing log(|Σ|)
+      virtual double logDetR() const;
 
     public:
 
@@ -266,15 +268,15 @@ namespace gtsam {
       /// Compute covariance matrix
       virtual Matrix covariance() const;
 
+      /// Compute the log of |Σ|
+      double logDeterminant() const;
+
       /**
-       * @brief Helper method to compute the normalization constant
-       * for a Gaussian noise model.
-       * k = 1/log(\sqrt(|2πΣ|))
+       * @brief Method to compute the normalization constant
+       * for a Gaussian noise model k = \sqrt(1/|2πΣ|).
+       * We compute this in the log-space for numerical accuracy,
+       * thus returning log(k).
        *
-       * We compute this in the log-space for numerical accuracy.
-       *
-       * @param noise_model The Gaussian noise model
-       * whose normalization constant we wish to compute.
        * @return double
        */
       double logNormalizationConstant() const;
@@ -308,11 +310,12 @@ namespace gtsam {
        */
       Vector sigmas_, invsigmas_, precisions_;
 
-    protected:
-
       /** constructor to allow for disabling initialization of invsigmas */
       Diagonal(const Vector& sigmas);
 
+      /// Compute the log of |R|. Used for computing log(|Σ|)
+      virtual double logDetR() const override;
+
     public:
       /** constructor - no initializations, for serialization */
       Diagonal();
@@ -545,6 +548,9 @@ namespace gtsam {
       Isotropic(size_t dim, double sigma) :
         Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
 
+      /// Compute the log of |R|. Used for computing log(|Σ|)
+      virtual double logDetR() const override;
+
     public:
 
       /* dummy constructor to allow for serialization */
@@ -608,6 +614,10 @@ namespace gtsam {
      * Unit: i.i.d. unit-variance noise on all m dimensions.
      */
     class GTSAM_EXPORT Unit : public Isotropic {
+    protected:
+      /// Compute the log of |R|. Used for computing log(|Σ|)
+      virtual double logDetR() const override;
+
     public:
 
       typedef std::shared_ptr<Unit> shared_ptr;