From 12e38a44e4f502abce324e886daef494c60b195d Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 1 Nov 2014 14:13:08 +0100
Subject: [PATCH] WriteableJacobianFactor will allow ExpressionFactor to write
 into the factor directly, (hopefull) eliminating huge overhead.

---
 .../nonlinear/tests/testExpressionFactor.cpp  | 55 +++++++++++++++++++
 1 file changed, 55 insertions(+)

diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
index b0900a43b..b5476bee9 100644
--- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
@@ -28,6 +28,9 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+
 using namespace std;
 using namespace gtsam;
 
@@ -420,6 +423,58 @@ TEST(ExpressionFactor, composeTernary) {
   EXPECT( assert_equal(expected, *jf,1e-9));
 }
 
+/* ************************************************************************* */
+
+/**
+ * Special version of JacobianFactor that allows Jacobians to be written
+ */
+class WriteableJacobianFactor: public JacobianFactor {
+
+  /**
+   *  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;
+  }
+  friend class ExpressionFactorWriteableJacobianFactorTest;
+  template<typename T>
+  friend class ExpressionFactor;
+};
+
+// Test Writeable JacobianFactor
+TEST(ExpressionFactor, WriteableJacobianFactor) {
+  std::list<size_t> keys = list_of(1)(2);
+  vector<size_t> dimensions(2);
+  dimensions[0] = 6;
+  dimensions[1] = 3;
+  SharedDiagonal model;
+  WriteableJacobianFactor 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));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;