From 36c921bb9f875c1b40986b41ab2073cb88e7fdf0 Mon Sep 17 00:00:00 2001
From: Stephen Williams <stephen.vincent.williams@gmail.com>
Date: Mon, 2 Apr 2012 20:06:00 +0000
Subject: [PATCH] Reordered gradient calculation to improve computation
 performance

---
 gtsam/nonlinear/ISAM2.h | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h
index f37d02312..8d3777d8c 100644
--- a/gtsam/nonlinear/ISAM2.h
+++ b/gtsam/nonlinear/ISAM2.h
@@ -216,7 +216,10 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional
     Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
     // Compute gradient contribution
     const ConditionalType& conditional(*result.first);
-    gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(),
+    // Rewrite -(R * P')'*d   as   -(d' * R * P')'   for computational speed reasons
+//    gradientContribution_ << -(conditional.get_R() * conditional.permutation().transpose()).transpose() * conditional.get_d(),
+//        -conditional.get_S().transpose() * conditional.get_d();
+    gradientContribution_ << -(conditional.get_d().transpose() * conditional.get_R() * conditional.permutation().transpose()).transpose(),
         -conditional.get_S().transpose() * conditional.get_d();
   }