From d0775faebaa732f2565484f41d7fe24374dfc959 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sat, 13 Jun 2015 12:26:10 -0700
Subject: [PATCH] Save slots to bring cost down from O(n^3) to O(n^2) - again,
 in theory. In practice, it did seem to help for larger HessianFactors (as
 expected).

---
 gtsam/linear/HessianFactor.cpp  | 8 +++++---
 gtsam/linear/JacobianFactor.cpp | 6 ++++--
 2 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index c071f8daa..7f3929488 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -350,11 +350,13 @@ void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
                                   SymmetricBlockMatrix* info) const {
   gttic(updateHessian_HessianFactor);
   // Apply updates to the upper triangle
-  DenseIndex n = size(), N = info->nBlocks()-1;
+  DenseIndex n = size(), N = info->nBlocks() - 1;
+  vector<DenseIndex> slots(n + 1);
   for (DenseIndex j = 0; j <= n; ++j) {
-    const DenseIndex J = (j==n) ? N :  Slot(infoKeys, keys_[j]);
+    const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
+    slots[j] = J;
     for (DenseIndex i = 0; i <= j; ++i) {
-      const DenseIndex I = (i==n) ? N : Slot(infoKeys, keys_[i]);
+      const DenseIndex I = slots[i];  // because i<=j, slots[i] is valid.
       (*info)(I, J) += info_(i, j);
     }
   }
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 5b90d913d..8214294b2 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -519,11 +519,13 @@ void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys,
 
     // Apply updates to the upper triangle
     // Loop over blocks of A, including RHS with j==n
+    vector<DenseIndex> slots(n+1);
     for (DenseIndex j = 0; j <= n; ++j) {
-      const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]);
+      const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]);
+      slots[j] = J;
       // Fill off-diagonal blocks with Ai'*Aj
       for (DenseIndex i = 0; i < j; ++i) {
-        const DenseIndex I = Slot(infoKeys, keys_[i]);
+        const DenseIndex I = slots[i];  // because i<j, slots[i] is valid.
         (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j);
       }
       // Fill diagonal block with Aj'*Aj