From b24af8554bf5f4aad8ac7cbb5626ae06c32e3d54 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Tue, 16 Jul 2013 20:22:05 +0000
Subject: [PATCH] Fixed whitening in BayesNet determinant

---
 gtsam/linear/GaussianBayesNetUnordered.cpp | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/GaussianBayesNetUnordered.cpp b/gtsam/linear/GaussianBayesNetUnordered.cpp
index 6909fcb9f..39658a483 100644
--- a/gtsam/linear/GaussianBayesNetUnordered.cpp
+++ b/gtsam/linear/GaussianBayesNetUnordered.cpp
@@ -134,8 +134,15 @@ namespace gtsam {
   double GaussianBayesNetUnordered::logDeterminant() const
   {
     double logDet = 0.0;
-    BOOST_FOREACH(const sharedConditional& cg, *this)
-      logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
+    BOOST_FOREACH(const sharedConditional& cg, *this) {
+      if(cg->get_model()) {
+        Vector diag = cg->get_R().diagonal();
+        cg->get_model()->whitenInPlace(diag);
+        logDet += diag.unaryExpr(ptr_fun<double,double>(log)).sum();
+      } else {
+        logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
+      }
+    }
     return logDet;
   }