Finalized batch and incremental timing scripts

tests/timeBatch.cpp

@@ -1,28 +1,31 @@
 /* ----------------------------------------------------------------------------
 
- * 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)
+* 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
- * -------------------------------------------------------------------------- */
+* See LICENSE for the license information
+* -------------------------------------------------------------------------- */
 
 /**
- * @file    timeBatch.cpp
- * @brief   Overall timing tests for batch solving
- * @author  Richard Roberts
- */
+* @file    timeBatch.cpp
+* @brief   Overall timing tests for batch solving
+* @author  Richard Roberts
+*/
 
 #include <gtsam/base/timing.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
 
 using namespace std;
 using namespace gtsam;
 
 int main(int argc, char *argv[]) {
 
+  try {
+
     cout << "Loading data..." << endl;
 
     string datasetFile = findExampleDataFile("w10000-odom");
@@ -46,9 +49,28 @@ int main(int argc, char *argv[]) {
       tictoc_finishedIteration_();
       tictoc_print_();
       cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl;
+      break;
     } while(!checkConvergence(optimizer.params().relativeErrorTol,
       optimizer.params().absoluteErrorTol, optimizer.params().errorTol,
       lastError, optimizer.error(), optimizer.params().verbosity));
 
+    // Compute marginals
+    Marginals marginals(graph, optimizer.values());
+    int i=0;
+    BOOST_FOREACH(Key key, initial.keys()) {
+      gttic_(marginalInformation);
+      Matrix info = marginals.marginalInformation(key);
+      gttoc_(marginalInformation);
+      tictoc_finishedIteration_();
+      if(i % 1000 == 0)
+        tictoc_print_();
+      ++i;
+    }
+
+  } catch(std::exception& e) {
+    cout << e.what() << endl;
+    return 1;
+  }
+
   return 0;
 }

tests/timeIncremental.cpp

@@ -23,12 +23,34 @@
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/Marginals.h>
 
+#include <fstream>
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/serialization/export.hpp>
+
 using namespace std;
 using namespace gtsam;
 using namespace gtsam::symbol_shorthand;
 
 typedef Pose2 Pose;
 
+typedef NoiseModelFactor1<Pose> NM1;
+typedef NoiseModelFactor2<Pose,Pose> NM2;
+
+BOOST_CLASS_EXPORT(Value);
+BOOST_CLASS_EXPORT(Pose);
+BOOST_CLASS_EXPORT(NonlinearFactor);
+BOOST_CLASS_EXPORT(NoiseModelFactor);
+BOOST_CLASS_EXPORT(NM1);
+BOOST_CLASS_EXPORT(NM2);
+BOOST_CLASS_EXPORT(BetweenFactor<Pose>);
+BOOST_CLASS_EXPORT(PriorFactor<Pose>);
+BOOST_CLASS_EXPORT(noiseModel::Base);
+BOOST_CLASS_EXPORT(noiseModel::Isotropic);
+BOOST_CLASS_EXPORT(noiseModel::Gaussian);
+BOOST_CLASS_EXPORT(noiseModel::Diagonal);
+BOOST_CLASS_EXPORT(noiseModel::Unit);
+
 double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
   // Compute degrees of freedom (observations - variables)
   // In ocaml, +1 was added to the observations to account for the prior, but
@@ -137,10 +159,45 @@ int main(int argc, char *argv[]) {
     }
   }
 
+  try {
+    {
+      std::ofstream writerStream("incremental_init", ios::binary);
+      boost::archive::binary_oarchive writer(writerStream);
+      writer << isam2.calculateEstimate();
+      writerStream.close();
+    }
+    {
+      std::ofstream writerStream("incremental_graph", ios::binary);
+      boost::archive::binary_oarchive writer(writerStream);
+      writer << isam2.getFactorsUnsafe();
+      writerStream.close();
+    }
+  } catch(std::exception& e) {
+    cout << e.what() << endl;
+  }
+
+  NonlinearFactorGraph graph;
+  Values values;
+
+  //{
+  //  std::ifstream readerStream("incremental_init", ios::binary);
+  //  boost::archive::binary_iarchive reader(readerStream);
+  //  reader >> values;
+  //}
+  //{
+  //  std::ifstream readerStream("incremental_graph", ios::binary);
+  //  boost::archive::binary_iarchive reader(readerStream);
+  //  reader >> graph;
+  //}
+
+  graph = isam2.getFactorsUnsafe();
+  values = isam2.calculateEstimate();
+
   // Compute marginals
-  Marginals marginals(isam2.getFactorsUnsafe(), isam2.calculateEstimate());
+  try {
+    Marginals marginals(graph, values);
     int i=0;
-  BOOST_FOREACH(Key key, initial.keys()) {
+    BOOST_FOREACH(Key key, values.keys()) {
       gttic_(marginalInformation);
       Matrix info = marginals.marginalInformation(key);
       gttoc_(marginalInformation);
@@ -149,6 +206,9 @@ int main(int argc, char *argv[]) {
         tictoc_print_();
       ++i;
     }
+  } catch(std::exception& e) {
+    cout << e.what() << endl;
+  }
 
   return 0;
 }