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 timeBatch.cpp
13* @brief Overall timing tests for batch solving
14* @author Richard Roberts
15*/
16
17#include <gtsam/base/timing.h>
18#include <gtsam/slam/dataset.h>
19#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
20#include <gtsam/nonlinear/Marginals.h>
21
22using namespace std;
23using namespace gtsam;
24
25int main(int argc, char *argv[]) {
26
27 try {
28
29 cout << "Loading data..." << endl;
30
31 string datasetFile = findExampleDataFile(name: "w10000");
32 auto [graph, initial] = load2D(filename: datasetFile);
33 graph->addPrior(key: 0, prior: initial->at<Pose2>(j: 0), model: noiseModel::Unit::Create(dim: 3));
34
35 cout << "Optimizing..." << endl;
36
37 gttic_(Create_optimizer);
38 LevenbergMarquardtOptimizer optimizer(*graph, *initial);
39 gttoc_(Create_optimizer);
40 tictoc_print_();
41 tictoc_reset_();
42 double lastError = optimizer.error();
43 do {
44 gttic_(Iterate_optimizer);
45 optimizer.iterate();
46 gttoc_(Iterate_optimizer);
47 tictoc_finishedIteration_();
48 tictoc_print_();
49 cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl;
50 break;
51 } while(!checkConvergence(relativeErrorTreshold: optimizer.params().relativeErrorTol,
52 absoluteErrorTreshold: optimizer.params().absoluteErrorTol, errorThreshold: optimizer.params().errorTol,
53 currentError: lastError, newError: optimizer.error(), verbosity: optimizer.params().verbosity));
54 tictoc_reset_();
55
56 // Compute marginals
57 gttic_(ConstructMarginals);
58 Marginals marginals(*graph, optimizer.values());
59 gttoc_(ConstructMarginals);
60 for(Key key: initial->keys()) {
61 gttic_(marginalInformation);
62 Matrix info = marginals.marginalInformation(variable: key);
63 gttoc_(marginalInformation);
64 tictoc_finishedIteration_();
65 }
66 tictoc_print_();
67
68 } catch(std::exception& e) {
69 cout << e.what() << endl;
70 return 1;
71 }
72
73 return 0;
74}
75