1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9 * -------------------------------------------------------------------------- */
10
11/**
12 * @file timeiSAM2Chain.cpp
13 * @brief Times each iteration of a long chain in iSAM2, to measure asymptotic performance
14 * @author Richard Roberts
15 */
16
17#include <gtsam/base/timing.h>
18#include <gtsam/slam/dataset.h>
19#include <gtsam/geometry/Pose2.h>
20#include <gtsam/slam/BetweenFactor.h>
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/nonlinear/ISAM2.h>
23#include <gtsam/nonlinear/Marginals.h>
24
25#include <fstream>
26#include <boost/archive/binary_oarchive.hpp>
27#include <boost/archive/binary_iarchive.hpp>
28#include <boost/serialization/export.hpp>
29#include <boost/random/mersenne_twister.hpp>
30#include <boost/random/uniform_real.hpp>
31#include <boost/random/variate_generator.hpp>
32
33using namespace std;
34using namespace gtsam;
35using namespace gtsam::symbol_shorthand;
36
37typedef Pose2 Pose;
38
39typedef NoiseModelFactorN<Pose> NM1;
40typedef NoiseModelFactorN<Pose,Pose> NM2;
41noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(dim: 3);
42
43int main(int argc, char *argv[]) {
44
45 const size_t steps = 50000;
46
47 cout << "Playing forward " << steps << " time steps..." << endl;
48
49 ISAM2 isam2;
50
51 // Times
52 vector<double> times(steps);
53
54 for(size_t step=0; step < steps; ++step) {
55
56 Values newVariables;
57 NonlinearFactorGraph newFactors;
58
59 // Collect measurements and new variables for the current step
60 gttic_(Create_measurements);
61 if(step == 0) {
62 // Add prior
63 newFactors.addPrior(key: 0, prior: Pose(), model: noiseModel::Unit::Create(dim: 3));
64 newVariables.insert(j: 0, val: Pose());
65 } else {
66 Vector eta = Vector::Random(size: 3) * 0.1;
67 Pose2 between = Pose().retract(v: eta);
68 // Add between
69 newFactors.add(factorOrContainer: BetweenFactor<Pose>(step-1, step, between, model));
70 newVariables.insert(j: step, val: isam2.calculateEstimate<Pose>(key: step-1) * between);
71 }
72
73 gttoc_(Create_measurements);
74
75 // Update iSAM2
76 gttic_(Update_ISAM2);
77 isam2.update(newFactors, newTheta: newVariables);
78 gttoc_(Update_ISAM2);
79
80 tictoc_finishedIteration_();
81
82 tictoc_getNode(node, Update_ISAM2);
83 times[step] = node->time();
84
85 if(step % 1000 == 0) {
86 cout << "Step " << step << endl;
87 tictoc_print_();
88 }
89 }
90
91 tictoc_print_();
92
93 // Write times
94 ofstream timesFile("times.txt");
95 for(double t: times) {
96 timesFile << t << "\n"; }
97
98 return 0;
99}
100