timeBatch.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
* 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
* -------------------------------------------------------------------------- */
 
#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");
    std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
      load2D(datasetFile);
 
    NonlinearFactorGraph graph = *data.first;
    Values initial = *data.second;
 
    cout << "Optimizing..." << endl;
 
    gttic_(Create_optimizer);
    LevenbergMarquardtOptimizer optimizer(graph, initial);
    gttoc_(Create_optimizer);
    tictoc_print_();
    double lastError = optimizer.error();
    do {
      gttic_(Iterate_optimizer);
      optimizer.iterate();
      gttoc_(Iterate_optimizer);
      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;
    for(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;
}