Lago timing

.cproject

@@ -2857,6 +2857,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="timeLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timeLago.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j4</buildArguments>

gtsam/nonlinear/lago.cpp

@@ -21,6 +21,7 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/timing.h>
 
 #include <boost/math/special_functions.hpp>
 
@@ -198,6 +199,7 @@ GaussianFactorGraph buildLinearOrientationGraph(
 /* ************************************************************************* */
 // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
 static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
+  gttic_(buildPose2graph);
   NonlinearFactorGraph pose2Graph;
 
   BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
@@ -250,6 +252,7 @@ static PredecessorMap<Key> findOdometricPath(
 // Return the orientations of a graph including only BetweenFactors<Pose2>
 static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
     bool useOdometricPath) {
+  gttic_(computeOrientations);
 
   // Find a minimum spanning tree
   PredecessorMap<Key> tree;
@@ -293,6 +296,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
 /* ************************************************************************* */
 Values computePoses(const NonlinearFactorGraph& pose2graph,
     VectorValues& orientationsLago) {
+  gttic_(computePoses);
 
   // Linearized graph on full poses
   GaussianFactorGraph linearPose2graph;

gtsam/nonlinear/tests/CMakeLists.txt

@@ -1 +1 @@
-gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam")
+gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam")

gtsam/nonlinear/tests/timeLago.cpp

@@ -0,0 +1,72 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    timeVirtual.cpp
+ * @brief   Time the overhead of using virtual destructors and methods
+ * @author  Richard Roberts
+ * @date    Dec 3, 2010
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/lago.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/base/timing.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char *argv[]) {
+
+  size_t trials = 1000;
+
+  // read graph
+  NonlinearFactorGraph g;
+  Values initial;
+  string inputFile = findExampleDataFile("noisyToyGraph");
+  readG2o(inputFile, g, initial);
+
+  // Add prior on the pose having index (key) = 0
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Sigmas(Vector3(0, 0, 0));
+  g.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+
+  // LAGO
+  for (size_t i = 0; i < trials; i++) {
+    {
+      gttic_(lago);
+
+      gttic_(init);
+      Values lagoInitial = lago::initialize(g);
+      gttoc_(init);
+
+      gttic_(refine);
+      GaussNewtonOptimizer optimizer(g, lagoInitial);
+      Values result = optimizer.optimize();
+      gttoc_(refine);
+    }
+
+    {
+      gttic_(optimize);
+      GaussNewtonOptimizer optimizer(g, initial);
+      Values result = optimizer.optimize();
+    }
+
+    tictoc_finishedIteration_();
+  }
+
+  tictoc_print_();
+
+  return 0;
+}