101 lines
2.7 KiB
C++
101 lines
2.7 KiB
C++
|
/* ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
* 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 timeiSAM2Chain.cpp
|
||
|
|
* @brief Times each iteration of a long chain in iSAM2, to measure asymptotic performance
|
||
|
|
* @author Richard Roberts
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <gtsam/base/timing.h>
|
||
|
|
#include <gtsam/slam/dataset.h>
|
||
|
|
#include <gtsam/geometry/Pose2.h>
|
||
|
|
#include <gtsam/slam/PriorFactor.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 NoiseModelFactor1<Pose> NM1;
|
||
|
|
typedef NoiseModelFactor2<Pose,Pose> NM2;
|
||
|
|
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim());
|
||
|
|
|
||
|
|
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.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
|
||
|
|
newVariables.insert(0, Pose());
|
||
|
|
} else {
|
||
|
|
Vector eta = Vector::Random(Pose::Dim()) * 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");
|
||
|
|
BOOST_FOREACH(double t, times) {
|
||
|
|
timesFile << t << "\n"; }
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|