From 898c06ccbbb93726711d8cab941a0df8a43907d1 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 15 Oct 2014 11:01:02 +0200
Subject: [PATCH] New multi-threaded, realistic SFM example (1M factors, not 1M
 calls on same factor)

---
 .cproject                                    |  8 ++
 gtsam_unstable/timing/timeSFMExpressions.cpp | 82 ++++++++++++++++++++
 2 files changed, 90 insertions(+)
 create mode 100644 gtsam_unstable/timing/timeSFMExpressions.cpp

diff --git a/.cproject b/.cproject
index 0665eaf06..7223156ff 100644
--- a/.cproject
+++ b/.cproject
@@ -912,6 +912,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="timeSFMExpressions.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timeSFMExpressions.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testCombinedImuFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
diff --git a/gtsam_unstable/timing/timeSFMExpressions.cpp b/gtsam_unstable/timing/timeSFMExpressions.cpp
new file mode 100644
index 000000000..fc2a8d97e
--- /dev/null
+++ b/gtsam_unstable/timing/timeSFMExpressions.cpp
@@ -0,0 +1,82 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    timeSFMExpressions.cpp
+ * @brief   time CalibratedCamera derivatives, realistic scenario
+ * @author  Frank Dellaert
+ * @date    October 3, 2014
+ */
+
+#include <gtsam_unstable/slam/expressions.h>
+#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+
+#include <time.h>
+#include <iostream>
+#include <iomanip>      // std::setprecision
+
+using namespace std;
+using namespace gtsam;
+
+//#define TERNARY
+
+int main() {
+
+  // number of cameras, and points
+  static const size_t M=100, N = 10000, n = M*N;
+
+  // Create leaves
+  Cal3_S2_ K('K', 0);
+  std::vector<Expression<Pose3> > x = createUnknowns<Pose3>(M, 'x');
+  std::vector<Expression<Point3> > p = createUnknowns<Point3>(N, 'p');
+
+  // Some parameters needed
+  Point2 z(-17, 30);
+  SharedNoiseModel model = noiseModel::Unit::Create(2);
+
+  // Create values
+  Values values;
+  values.insert(Symbol('K', 0), Cal3_S2());
+  for (int i = 0; i < M; i++)
+    values.insert(Symbol('x', i), Pose3());
+  for (int j = 0; j < N; j++)
+    values.insert(Symbol('p', j), Point3(0, 0, 1));
+
+  long timeLog = clock();
+  NonlinearFactorGraph graph;
+  for (int i = 0; i < M; i++) {
+    for (int j = 0; j < N; j++) {
+      NonlinearFactor::shared_ptr f = boost::make_shared<
+          ExpressionFactor<Point2> >
+#ifdef TERNARY
+          (model, z, project3(x[i], p[j], K));
+#else
+          (model, z, uncalibrate(K, project(transform_to(x[i], p[j]))));
+#endif
+      graph.push_back(f);
+    }
+  }
+  long timeLog2 = clock();
+  cout << setprecision(3);
+  double seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC;
+  cout << seconds << " seconds to build" << endl;
+
+  timeLog = clock();
+  GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
+  timeLog2 = clock();
+  seconds = (double) (timeLog2 - timeLog) / CLOCKS_PER_SEC;
+  cout << seconds << " seconds to linearize" << endl;
+  cout << ((double) seconds * 1000000 / n) << " musecs/call" << endl;
+
+  return 0;
+}