From cb69f2cb8285b78d4f2b382a5fca5449f909891a Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 2 Nov 2014 11:40:48 +0100
Subject: [PATCH] Fastest linearize so far. Putting 'unsafe' constructor in
 JacobianFactor itself makes a *huge* difference.

---
 gtsam/linear/JacobianFactor.h                 | 19 +++++++
 gtsam_unstable/nonlinear/ExpressionFactor.h   | 55 +------------------
 .../nonlinear/tests/testExpressionFactor.cpp  |  4 +-
 3 files changed, 24 insertions(+), 54 deletions(-)

diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index b90012822..d98596a9f 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -140,6 +140,25 @@ namespace gtsam {
     JacobianFactor(
       const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
 
+    /**
+     *  Constructor
+     *  @param keys in some order
+     *  @param diemnsions of the variables in same order
+     *  @param m output dimension
+     *  @param model noise model (default NULL)
+     */
+    template<class KEYS, class DIMENSIONS>
+    JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
+        const SharedDiagonal& model = SharedDiagonal()) :
+        Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
+      Ab_.matrix().setZero();
+    }
+
+    /// Direct access to VerticalBlockMatrix
+    VerticalBlockMatrix& Ab() {
+      return Ab_;
+    }
+
     /**
      * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
      * structure computed for \c graph is already available, providing it will reduce the amount of
diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index aaf10dc98..0394fbe5c 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -24,57 +24,8 @@
 #include <boost/range/algorithm.hpp>
 #include <numeric>
 
-class ExpressionFactorWriteableJacobianFactorTest;
-
 namespace gtsam {
 
-/**
- * Special version of JacobianFactor that allows Jacobians to be written
- * Eliminates a large proportion of overhead
- * Note all ExpressionFactor<T> are friends, not for general consumption.
- */
-class WriteableJacobianFactor: public JacobianFactor {
-
-public:
-
-  /**
-   *  Constructor
-   *  @param keys in some order
-   *  @param diemnsions of the variables in same order
-   *  @param m output dimension
-   *  @param model noise model (default NULL)
-   */
-  template<class KEYS, class DIMENSIONS>
-  WriteableJacobianFactor(const KEYS& keys, const DIMENSIONS& dims,
-      DenseIndex m, const SharedDiagonal& model = SharedDiagonal()) {
-
-    // Check noise model dimension
-    if (model && (DenseIndex) model->dim() != m)
-      throw InvalidNoiseModel(m, model->dim());
-
-    // copy the keys
-    keys_.resize(keys.size());
-    std::copy(keys.begin(), keys.end(), keys_.begin());
-
-    // Check number of variables
-    if (dims.size() != keys_.size())
-      throw std::invalid_argument(
-          "WriteableJacobianFactor: size of dimensions and keys do not agree.");
-
-    Ab_ = VerticalBlockMatrix(dims.begin(), dims.end(), m, true);
-    Ab_.matrix().setZero();
-    model_ = model;
-  }
-
-  VerticalBlockMatrix& Ab() {
-    return Ab_;
-  }
-
-//  friend class ::ExpressionFactorWriteableJacobianFactorTest;
-//  template<typename T>
-//  friend class ExpressionFactor;
-};
-
 /**
  * Factor that supports arbitrary expressions via AD
  */
@@ -164,9 +115,9 @@ public:
       model = constrained->unit();
 
     // Create a writeable JacobianFactor in advance
-    boost::shared_ptr<WriteableJacobianFactor> factor = boost::make_shared<
-        WriteableJacobianFactor>(keys_, dimensions_,
-        traits::dimension<T>::value, model);
+    boost::shared_ptr<JacobianFactor> factor(
+        new JacobianFactor(keys_, dimensions_, traits::dimension<T>::value,
+            model));
 
     // Wrap keys and VerticalBlockMatrix into structure passed to expression_
     JacobianMap map(keys_, factor->Ab());
diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
index f9a4bc532..08496e5ff 100644
--- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
@@ -431,9 +431,9 @@ TEST(ExpressionFactor, WriteableJacobianFactor) {
   dimensions[0] = 6;
   dimensions[1] = 3;
   SharedDiagonal model;
-  WriteableJacobianFactor actual(keys, dimensions, 2, model);
+  JacobianFactor actual(keys, dimensions, 2, model);
   JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2));
-  EXPECT( assert_equal(expected, *(JacobianFactor*)(&actual),1e-9));
+  EXPECT( assert_equal(expected, actual,1e-9));
 }
 
 /* ************************************************************************* */