diff --git a/tests/timeiSAM2Chain.cpp b/tests/timeiSAM2Chain.cpp new file mode 100644 index 000000000..cad75e17a --- /dev/null +++ b/tests/timeiSAM2Chain.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +typedef Pose2 Pose; + +typedef NoiseModelFactor1 NM1; +typedef NoiseModelFactor2 NM2; +noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim()); + +int main(int argc, char *argv[]) { + + const size_t steps = 10000; + + cout << "Playing forward " << steps << " time steps..." << endl; + + ISAM2 isam2; + + // Times + vector 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(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(step-1, step, between, model)); + newVariables.insert(step, isam2.calculateEstimate(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; +}