From 55bebdc72daca9b9f47910affb52d4dd5c552990 Mon Sep 17 00:00:00 2001
From: Frank <frank@skyd.io>
Date: Thu, 25 Feb 2016 16:53:17 -0800
Subject: [PATCH] Got rid of obsolete types and mate FBlocks a mutable cache

---
 gtsam/slam/SmartFactorBase.h | 33 +++++++++++----------------------
 1 file changed, 11 insertions(+), 22 deletions(-)

diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 51e53769e..eca7f531b 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -50,6 +50,12 @@ private:
   typedef SmartFactorBase<CAMERA> This;
   typedef typename CAMERA::Measurement Z;
 
+public:
+
+  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
+  static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
+  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
+
 protected:
   /**
    * As of Feb 22, 2015, the noise model is the same for all measurements and
@@ -70,24 +76,11 @@ protected:
   const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
   /// @}
 
-  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
-  static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
-
-  // Definitions for block matrices used internally
-  typedef Eigen::Matrix<double, Dim, ZDim> MatrixD2; // F'
-  typedef Eigen::Matrix<double, Dim, Dim> MatrixDD; // camera hessian block
-  typedef Eigen::Matrix<double, ZDim, 3> Matrix23;
-  typedef Eigen::Matrix<double, Dim, 1> VectorD;
-  typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
-
   // Cache for Fblocks, to avoid a malloc ever time we re-linearize
   mutable std::vector<MatrixZD> Fblocks;
 
 public:
 
-  // Definitions for blocks of F, externally visible
-  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
-
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for a smart pointer to a factor
@@ -101,10 +94,9 @@ public:
 
   /// Constructor
   SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
-      boost::optional<Pose3> body_P_sensor = boost::none, size_t expectedNumberCameras=10) :
-          body_P_sensor_(body_P_sensor) {
-
-    Fblocks.reserve(expectedNumberCameras);
+                  boost::optional<Pose3> body_P_sensor = boost::none,
+                  size_t expectedNumberCameras = 10)
+      : body_P_sensor_(body_P_sensor), Fblocks(expectedNumberCameras) {
 
     if (!sharedNoiseModel)
       throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
@@ -293,8 +285,7 @@ public:
     computeJacobians(Fblocks, E, b, cameras, point);
 
     // build augmented hessian
-    SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E,
-        b);
+    SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fblocks, E, b);
 
     return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
         augmentedHessian);
@@ -311,10 +302,8 @@ public:
       const FastVector<Key> allKeys) const {
     Matrix E;
     Vector b;
-    std::vector<MatrixZD> Fblocks;
     computeJacobians(Fblocks, E, b, cameras, point);
-    Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_,
-        augmentedHessian);
+    Cameras::UpdateSchurComplement(Fblocks, E, b, allKeys, keys_, augmentedHessian);
   }
 
   /// Whiten the Jacobians computed by computeJacobians using noiseModel_