From e0f6570f8fe63a5969ad65f8c2774b0cdbf780ab Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 7 Jun 2015 20:53:54 -0700
Subject: [PATCH] Timing script that takes BAL file as input. Compile with
 BUILD_TYPE=Timing

---
 timing/timeSFMBAL.cpp | 67 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 67 insertions(+)
 create mode 100644 timing/timeSFMBAL.cpp

diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp
new file mode 100644
index 000000000..49bf23024
--- /dev/null
+++ b/timing/timeSFMBAL.cpp
@@ -0,0 +1,67 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    timeSFMBAL.cpp
+ * @brief   time structure from motion with BAL file
+ * @author  Frank Dellaert
+ * @date    June 6, 2015
+ */
+
+#include <boost/foreach.hpp>
+#include <gtsam/base/timing.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/inference/FactorGraph.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <stddef.h>
+#include <stdexcept>
+#include <string>
+
+using namespace std;
+using namespace gtsam;
+
+//#define TERNARY
+
+int main(int argc, char *argv[]) {
+  typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
+  using symbol_shorthand::P;
+
+  string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
+  SfM_data db;
+  bool success = readBAL(argc>1 ? argv[1] : defaultFilename, db);
+  if (!success) throw runtime_error("Could not access file!");
+
+  SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
+  NonlinearFactorGraph graph;
+
+  for (size_t j = 0; j < db.number_tracks(); j++) {
+    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements)
+      graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
+  }
+
+  Values initial = initialCamerasAndPointsEstimate(db);
+
+  LevenbergMarquardtOptimizer lm(graph, initial);
+
+  Values actual = lm.optimize();
+  tictoc_print_();
+
+  return 0;
+}