From 784e9f575571ed5acd95df21e24fa332e3935e38 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 28 Sep 2012 17:48:49 +0000 Subject: [PATCH] Added overall timing scripts for batch and incremental (ISAM2) solving --- tests/timeBatch.cpp | 54 +++++++++++++++ tests/timeIncremental.cpp | 140 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 194 insertions(+) create mode 100644 tests/timeBatch.cpp create mode 100644 tests/timeIncremental.cpp diff --git a/tests/timeBatch.cpp b/tests/timeBatch.cpp new file mode 100644 index 000000000..0548d7f94 --- /dev/null +++ b/tests/timeBatch.cpp @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeBatch.cpp + * @brief Overall timing tests for batch solving + * @author Richard Roberts + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char *argv[]) { + + cout << "Loading data..." << endl; + + string datasetFile = findExampleDataFile("w10000-odom"); + std::pair data = + load2D(datasetFile); + + NonlinearFactorGraph graph = *data.first; + Values initial = *data.second; + + cout << "Optimizing..." << endl; + + tic_(1, "Create optimizer"); + LevenbergMarquardtOptimizer optimizer(graph, initial); + toc_(1, "Create optimizer"); + tictoc_print_(); + double lastError = optimizer.error(); + do { + tic_(2, "Iterate optimizer"); + optimizer.iterate(); + toc_(2, "Iterate optimizer"); + tictoc_finishedIteration_(); + tictoc_print_(); + cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl; + } while(!checkConvergence(optimizer.params().relativeErrorTol, + optimizer.params().absoluteErrorTol, optimizer.params().errorTol, + lastError, optimizer.error(), optimizer.params().verbosity)); + + return 0; +} diff --git a/tests/timeIncremental.cpp b/tests/timeIncremental.cpp new file mode 100644 index 000000000..333d0e777 --- /dev/null +++ b/tests/timeIncremental.cpp @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeIncremental.cpp + * @brief Overall timing tests for incremental solving + * @author Richard Roberts + */ + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +typedef Pose2 Pose; + +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 + // the factor graph already includes a factor for the prior/equality constraint. + // double dof = graph.size() - config.size(); + int graph_dim = 0; + BOOST_FOREACH(const boost::shared_ptr& nlf, graph) { + graph_dim += nlf->dim(); + } + double dof = graph_dim - config.dim(); // kaess: changed to dim + return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error +} + +int main(int argc, char *argv[]) { + + cout << "Loading data..." << endl; + + tic_(1, "Find datafile (script only)"); + string datasetFile = findExampleDataFile("w10000-odom"); + std::pair data = + load2D(datasetFile); + toc_(1, "Find datafile (script only)"); + + NonlinearFactorGraph measurements = *data.first; + Values initial = *data.second; + + cout << "Playing forward time steps..." << endl; + + ISAM2 isam2; + + size_t nextMeasurement = 0; + for(size_t step=1; nextMeasurement < measurements.size(); ++step) { + + Values newVariables; + NonlinearFactorGraph newFactors; + + // Collect measurements and new variables for the current step + tic_(2, "Collect measurements (script only)"); + if(step == 1) { + // cout << "Initializing " << 0 << endl; + newVariables.insert(0, Pose()); + // Add prior + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + } + while(nextMeasurement < measurements.size()) { + + NonlinearFactor::shared_ptr measurementf = measurements[nextMeasurement]; + + if(BetweenFactor::shared_ptr measurement = + boost::dynamic_pointer_cast >(measurementf)) + { + // Stop collecting measurements that are for future steps + if(measurement->key1() > step || measurement->key2() > step) + break; + + // Require that one of the nodes is the current one + if(measurement->key1() != step && measurement->key2() != step) + throw runtime_error("Problem in data file, out-of-sequence measurements"); + + // Add a new factor + newFactors.push_back(measurement); + + // Initialize the new variable + if(measurement->key1() == step && measurement->key2() == step-1) { + if(step == 1) + newVariables.insert(step, measurement->measured().inverse()); + else { + Pose prevPose = isam2.calculateEstimate(step-1); + newVariables.insert(step, prevPose * measurement->measured().inverse()); + } + // cout << "Initializing " << step << endl; + } else if(measurement->key2() == step && measurement->key1() == step-1) { + if(step == 1) + newVariables.insert(step, measurement->measured()); + else { + Pose prevPose = isam2.calculateEstimate(step-1); + newVariables.insert(step, prevPose * measurement->measured()); + } + // cout << "Initializing " << step << endl; + } + } else { + throw std::runtime_error("Unknown factor type read from data file"); + } + ++ nextMeasurement; + } + toc_(2, "Collect measurements (script only)"); + + // Update iSAM2 + tic_(3, "Update ISAM2"); + isam2.update(newFactors, newVariables); + toc_(3, "Update ISAM2"); + + if(step % 20 == 0) { + tic_(4, "chi2 (script only)"); + Values estimate(isam2.calculateEstimate()); + double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate); + cout << "chi2 = " << chi2 << endl; + toc_(4, "chi2 (script only)"); + } + + if(step % 100 == 0) { + cout << "Step " << step << endl; + tictoc_print_(); + } + + tictoc_finishedIteration_(); + } + + return 0; +}