timeiSAM2Chain.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/geometry/Pose2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#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>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
 
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
 
typedef Pose2 Pose;
 
typedef NoiseModelFactorN<Pose> NM1;
typedef NoiseModelFactorN<Pose,Pose> NM2;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
 
int main(int argc, char *argv[]) {
 
  const size_t steps = 50000;
 
  cout << "Playing forward " << steps << " time steps..." << endl;
 
  ISAM2 isam2;
 
  // Times
  vector<double> times(steps);
 
  for(size_t step=0; step < steps; ++step) {
 
    Values newVariables;
    NonlinearFactorGraph newFactors;
 
    // Collect measurements and new variables for the current step
    gttic_(Create_measurements);
    if(step == 0) {
      // Add prior
      newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3));
      newVariables.insert(0, Pose());
    } else {
      Vector eta = Vector::Random(3) * 0.1;
      Pose2 between = Pose().retract(eta);
      // Add between
      newFactors.add(BetweenFactor<Pose>(step-1, step, between, model));
      newVariables.insert(step, isam2.calculateEstimate<Pose>(step-1) * between);
    }
 
    gttoc_(Create_measurements);
 
    // Update iSAM2
    gttic_(Update_ISAM2);
    isam2.update(newFactors, newVariables);
    gttoc_(Update_ISAM2);
 
    tictoc_finishedIteration_();
 
    tictoc_getNode(node, Update_ISAM2);
    times[step] = node->time();
 
    if(step % 1000 == 0) {
      cout << "Step " << step << endl;
      tictoc_print_();
    }
  }
 
  tictoc_print_();
 
  // Write times
  ofstream timesFile("times.txt");
  for(double t: times) {
    timesFile << t << "\n"; }
 
  return 0;
}