From 4c8ba55d0e8e2fddcf02c27057c65e878473b01d Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 26 Feb 2016 08:28:16 -0800
Subject: [PATCH] Added smart timing

---
 timing/timeSFMBAL.h          |  9 ++++--
 timing/timeSFMBALnavTcam.cpp |  2 +-
 timing/timeSFMBALsmart.cpp   | 55 ++++++++++++++++++++++++++++++++++++
 3 files changed, 62 insertions(+), 4 deletions(-)
 create mode 100644 timing/timeSFMBALsmart.cpp

diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h
index 24b67557a..1642af7e8 100644
--- a/timing/timeSFMBAL.h
+++ b/timing/timeSFMBAL.h
@@ -67,7 +67,7 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
   // Set parameters to be similar to ceres
   LevenbergMarquardtParams params;
   LevenbergMarquardtParams::SetCeresDefaults(&params);
-  params.setVerbosityLM("SUMMARY");
+//  params.setVerbosityLM("SUMMARY");
 
   if (gUseSchur) {
     // Create Schur-complement ordering
@@ -81,8 +81,11 @@ int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
   }
 
   // Optimize
-  LevenbergMarquardtOptimizer lm(graph, initial, params);
-  Values result = lm.optimize();
+  {
+    gttic_(optimize);
+    LevenbergMarquardtOptimizer lm(graph, initial, params);
+    Values result = lm.optimize();
+  }
 
   tictoc_finishedIteration_();
   tictoc_print_();
diff --git a/timing/timeSFMBALnavTcam.cpp b/timing/timeSFMBALnavTcam.cpp
index e370a5a67..0d0eb4f65 100644
--- a/timing/timeSFMBALnavTcam.cpp
+++ b/timing/timeSFMBALnavTcam.cpp
@@ -35,12 +35,12 @@ int main(int argc, char* argv[]) {
   // Build graph using conventional GeneralSFMFactor
   NonlinearFactorGraph graph;
   for (size_t j = 0; j < db.number_tracks(); j++) {
+    Point3_ nav_point_(P(j));
     BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
       size_t i = m.first;
       Point2 z = m.second;
       Pose3_ navTcam_(C(i));
       Cal3Bundler_ calibration_(K(i));
-      Point3_ nav_point_(P(j));
       graph.addExpressionFactor(
           gNoiseModel, z,
           uncalibrate(calibration_,
diff --git a/timing/timeSFMBALsmart.cpp b/timing/timeSFMBALsmart.cpp
new file mode 100644
index 000000000..708c25695
--- /dev/null
+++ b/timing/timeSFMBALsmart.cpp
@@ -0,0 +1,55 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    timeSFMBALsmart.cpp
+ * @brief   time SFM with BAL file,  SmartProjectionFactor
+ * @author  Frank Dellaert
+ * @date    Feb 26, 2016
+ */
+
+#include "timeSFMBAL.h"
+
+#include <gtsam/slam/SmartProjectionFactor.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+
+using namespace std;
+using namespace gtsam;
+
+typedef PinholeCamera<Cal3Bundler> Camera;
+typedef SmartProjectionFactor<Camera> SfmFactor;
+
+int main(int argc, char* argv[]) {
+  // parse options and read BAL file
+  SfM_data db = preamble(argc, argv);
+
+  // Add smart factors to graph
+  NonlinearFactorGraph graph;
+  for (size_t j = 0; j < db.number_tracks(); j++) {
+    auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
+    for (const SfM_Measurement& m : db.tracks[j].measurements) {
+      size_t i = m.first;
+      Point2 z = m.second;
+      smartFactor->add(z, C(i));
+    }
+    graph.push_back(smartFactor);
+  }
+
+  Values initial;
+  size_t i = 0;
+  gUseSchur = false;
+  for (const SfM_Camera& camera : db.cameras)
+    initial.insert(C(i++), camera);
+
+  return optimize(db, graph, initial);
+}