From 29e450dc009eeaa43619080ccdf67235058b95a5 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 2 Aug 2013 02:04:40 +0000
Subject: [PATCH] Working on HessianFactor

---
 gtsam/base/SymmetricBlockMatrix.h  |  2 ++
 gtsam/base/VerticalBlockMatrix.cpp | 14 ++++++++++++++
 gtsam/base/VerticalBlockMatrix.h   | 19 ++++++++++++-------
 gtsam/linear/HessianFactor.cpp     | 26 +++++++-------------------
 4 files changed, 35 insertions(+), 26 deletions(-)

diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h
index 281418c90..fd9ec42f1 100644
--- a/gtsam/base/SymmetricBlockMatrix.h
+++ b/gtsam/base/SymmetricBlockMatrix.h
@@ -214,6 +214,8 @@ namespace gtsam {
       }
     }
 
+    friend class VerticalBlockMatrix;
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;
diff --git a/gtsam/base/VerticalBlockMatrix.cpp b/gtsam/base/VerticalBlockMatrix.cpp
index 1eb8c01a7..8d9598bf4 100644
--- a/gtsam/base/VerticalBlockMatrix.cpp
+++ b/gtsam/base/VerticalBlockMatrix.cpp
@@ -17,6 +17,7 @@
  * @date    Sep 18, 2010 */
 
 #include <gtsam/base/VerticalBlockMatrix.h>
+#include <gtsam/base/SymmetricBlockMatrix.h>
 
 namespace gtsam {
 
@@ -33,4 +34,17 @@ namespace gtsam {
     return result;
   }
 
+  /* ************************************************************************* */
+  VerticalBlockMatrix VerticalBlockMatrix::LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height)
+  {
+    VerticalBlockMatrix result;
+    result.variableColOffsets_.resize(rhs.nBlocks() + 1);
+    std::copy(rhs.variableColOffsets_.begin() + rhs.blockStart_, rhs.variableColOffsets_.end(),
+      result.variableColOffsets_.begin());
+    result.matrix_.resize(height, result.variableColOffsets_.back());
+    result.rowEnd_ = height;
+    result.assertInvariants();
+    return result;
+  }
+
 }
diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h
index 11c4d14d2..502d411d8 100644
--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@@ -93,6 +93,18 @@ namespace gtsam {
       matrix_.resize(height, variableColOffsets_.back());
       assertInvariants();
     }
+    
+    /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
+    *  If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
+    *  the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
+    *  rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the
+    *  underlying matrix will be the size of the view of the source matrix.  */
+    static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
+
+    /** Copy the block structure, but do not copy the matrix data. If blockStart() has been
+    *   modified, this copies the structure of the corresponding matrix view. In the destination
+    *   VerticalBlockMatrix, blockStart() will be 0. */
+    static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height);
 
     /// Row size
     DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
@@ -173,13 +185,6 @@ namespace gtsam {
     /** Non-const access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
     Matrix& matrix() { return matrix_; }
 
-    /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
-    *  If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
-    *  the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
-    *  rowStart() will thus be 0, rowEnd() will be cols() of the source VerticalBlockView, and the
-    *  underlying matrix will be the size of the view of the source matrix.  */
-    static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
-
   protected:
     void assertInvariants() const {
       assert(matrix_.cols() == variableColOffsets_.back());
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index bd1b943bb..6b5f8581e 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -393,32 +393,20 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr
 {
   static const bool debug = false;
 
-  // Extract conditionals
-  gttic(extract_conditionals);
-  typedef VerticalBlockView<Matrix> BlockAb;
-  BlockAb Ab(matrix_, info_);
-
-  size_t varDim = info_.offset(nrFrontals);
-  Ab.rowEnd() = Ab.rowStart() + varDim;
-
   // Create one big conditionals with many frontal variables.
-  gttic(construct_cond);
-
-  VerticalBlockMatrix Ab()
+  gttic(Construct_conditional);
+  size_t varDim = info_.offset(nrFrontals);
+  VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(info_, varDim);
   GaussianConditional::shared_ptr conditional = boost::make_shared<GaussianConditional>(
     keys_, nrFrontals, Ab);
-  gttoc(construct_cond);
-
-  gttoc(extract_conditionals);
+  gttoc(Construct_conditional);
 
+  gttic(Remaining_factor);
   // Take lower-right block of Ab_ to get the new factor
-  gttic(remaining_factor);
   info_.blockStart() = nrFrontals;
   // Assign the keys
-  vector<Index> remainingKeys(keys_.size() - nrFrontals);
-  remainingKeys.assign(keys_.begin() + nrFrontals, keys_.end());
-  keys_.swap(remainingKeys);
-  gttoc(remaining_factor);
+  keys_.erase(begin(), begin() + nrFrontals);
+  gttoc(Remaining_factor);
 
   return conditional;
 }