From 6365dbaba58bcfb59011af3334543417ec94d915 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 22 Feb 2015 14:03:13 +0100
Subject: [PATCH] Moved from smartfactors_ceres

---
 timing/DummyFactor.h        |  56 +++++++++++++
 timing/timeSchurFactors.cpp | 152 ++++++++++++++++++++++++++++++++++++
 2 files changed, 208 insertions(+)
 create mode 100644 timing/DummyFactor.h
 create mode 100644 timing/timeSchurFactors.cpp

diff --git a/timing/DummyFactor.h b/timing/DummyFactor.h
new file mode 100644
index 000000000..ff9732909
--- /dev/null
+++ b/timing/DummyFactor.h
@@ -0,0 +1,56 @@
+/**
+ * @file    DummyFactor.h
+ * @brief   Just to help in timing overhead
+ * @author  Frank Dellaert
+ */
+
+#pragma once
+
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
+
+namespace gtsam {
+
+/**
+ * DummyFactor
+ */
+template<size_t D> //
+class DummyFactor: public RegularImplicitSchurFactor<D> {
+
+public:
+
+  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
+
+  DummyFactor() {
+  }
+
+  DummyFactor(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
+      const Matrix3& P, const Vector& b) :RegularImplicitSchurFactor<D>(Fblocks,E,P,b)
+       {
+  }
+
+  virtual ~DummyFactor() {
+  }
+
+public:
+
+  /**
+   * @brief Dummy version to measure overhead of key access
+   */
+  void multiplyHessian(double alpha, const VectorValues& x,
+      VectorValues& y) const {
+
+    BOOST_FOREACH(const KeyMatrix2D& Fi, this->Fblocks_) {
+      static const Vector empty;
+      Key key = Fi.first;
+      std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
+      Vector& yi = it.first->second;
+      yi = x.at(key);
+    }
+  }
+
+};
+// DummyFactor
+
+}
+
diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp
new file mode 100644
index 000000000..06a526567
--- /dev/null
+++ b/timing/timeSchurFactors.cpp
@@ -0,0 +1,152 @@
+/**
+ * @file    timeSchurFactors.cpp
+ * @brief   Time various Schur-complement Jacobian factors
+ * @author  Frank Dellaert
+ * @date    Oct 27, 2013
+ */
+
+#include "DummyFactor.h"
+#include <gtsam/base/timing.h>
+
+#include <gtsam/slam/JacobianFactorQ.h>
+#include "gtsam/slam/JacobianFactorQR.h"
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
+
+#include <boost/assign/list_of.hpp>
+#include <boost/assign/std/vector.hpp>
+#include <fstream>
+
+using namespace std;
+using namespace boost::assign;
+using namespace gtsam;
+
+#define SLOW
+#define RAW
+#define HESSIAN
+#define NUM_ITERATIONS 1000
+
+// Create CSV file for results
+ofstream os("timeSchurFactors.csv");
+
+/*************************************************************************************/
+template<size_t D>
+void timeAll(size_t m, size_t N) {
+
+  cout << m << endl;
+
+  // create F
+  typedef Eigen::Matrix<double, 2, D> Matrix2D;
+  typedef pair<Key, Matrix2D> KeyMatrix2D;
+  vector < pair<Key, Matrix2D> > Fblocks;
+  for (size_t i = 0; i < m; i++)
+    Fblocks.push_back(KeyMatrix2D(i, (i + 1) * Matrix::Ones(2, D)));
+
+  // create E
+  Matrix E(2 * m, 3);
+  for (size_t i = 0; i < m; i++)
+    E.block < 2, 3 > (2 * i, 0) = Matrix::Ones(2, 3);
+
+  // Calculate point covariance
+  Matrix P = (E.transpose() * E).inverse();
+
+  // RHS and sigmas
+  const Vector b = gtsam::repeat(2 * m, 1);
+  const SharedDiagonal model;
+
+  // parameters for multiplyHessianAdd
+  double alpha = 0.5;
+  VectorValues xvalues, yvalues;
+  for (size_t i = 0; i < m; i++)
+    xvalues.insert(i, gtsam::repeat(D, 2));
+
+  // Implicit
+  RegularImplicitSchurFactor<D> implicitFactor(Fblocks, E, P, b);
+  // JacobianFactor with same error
+  JacobianFactorQ<D, 2> jf(Fblocks, E, P, b, model);
+  // JacobianFactorQR with same error
+  JacobianFactorQR<D, 2> jqr(Fblocks, E, P, b, model);
+  // Hessian
+  HessianFactor hessianFactor(jqr);
+
+#define TIME(label,factor,xx,yy) {\
+     for (size_t t = 0; t < N; t++) \
+     factor.multiplyHessianAdd(alpha, xx, yy);\
+     gttic_(label);\
+     for (size_t t = 0; t < N; t++) {\
+       factor.multiplyHessianAdd(alpha, xx, yy);\
+     }\
+     gttoc_(label);\
+     tictoc_getNode(timer, label)\
+     os << timer->secs()/NUM_ITERATIONS << ", ";\
+   }
+
+#ifdef SLOW
+  TIME(Implicit, implicitFactor, xvalues, yvalues)
+  TIME(Jacobian, jf, xvalues, yvalues)
+  TIME(JacobianQR, jqr, xvalues, yvalues)
+#endif
+
+#ifdef HESSIAN
+  TIME(Hessian, hessianFactor, xvalues, yvalues)
+#endif
+
+#ifdef OVERHEAD
+  DummyFactor<D> dummy(Fblocks, E, P, b);
+  TIME(Overhead,dummy,xvalues,yvalues)
+#endif
+
+#ifdef RAW
+  { // Raw memory Version
+    FastVector < Key > keys;
+    for (size_t i = 0; i < m; i++)
+      keys += i;
+    Vector x = xvalues.vector(keys);
+    double* xdata = x.data();
+
+    // create a y
+    Vector y = zero(m * D);
+    TIME(RawImplicit, implicitFactor, xdata, y.data())
+    TIME(RawJacobianQ, jf, xdata, y.data())
+    TIME(RawJacobianQR, jqr, xdata, y.data())
+  }
+#endif
+
+  os << m << endl;
+
+} // timeAll
+
+/*************************************************************************************/
+int main(void) {
+#ifdef SLOW
+  os << "Implicit,";
+  os << "JacobianQ,";
+  os << "JacobianQR,";
+#endif
+#ifdef HESSIAN
+  os << "Hessian,";
+#endif
+#ifdef OVERHEAD
+  os << "Overhead,";
+#endif
+#ifdef RAW
+  os << "RawImplicit,";
+  os << "RawJacobianQ,";
+  os << "RawJacobianQR,";
+#endif
+  os << "m" << endl;
+  // define images
+  vector < size_t > ms;
+  //  ms += 2;
+  //  ms += 3, 4, 5, 6, 7, 8, 9, 10;
+  // ms += 11,12,13,14,15,16,17,18,19;
+  //  ms += 20, 30, 40, 50;
+  // ms += 20,30,40,50,60,70,80,90,100;
+  for (size_t m = 2; m <= 50; m += 2)
+    ms += m;
+  //for (size_t m=10;m<=100;m+=10) ms += m;
+  // loop over number of images
+  BOOST_FOREACH(size_t m,ms)
+    timeAll<6>(m, NUM_ITERATIONS);
+}
+
+//*************************************************************************************