timeBatch.cpp
Go to the documentation of this file.
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 
17 #include <gtsam/base/timing.h>
18 #include <gtsam/slam/dataset.h>
21 
22 using namespace std;
23 using namespace gtsam;
24 
25 int main(int argc, char *argv[]) {
26 
27  try {
28 
29  cout << "Loading data..." << endl;
30 
31  string datasetFile = findExampleDataFile("w10000-odom");
32  std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
33  load2D(datasetFile);
34 
35  NonlinearFactorGraph graph = *data.first;
36  Values initial = *data.second;
37 
38  cout << "Optimizing..." << endl;
39 
40  gttic_(Create_optimizer);
41  LevenbergMarquardtOptimizer optimizer(graph, initial);
42  gttoc_(Create_optimizer);
43  tictoc_print_();
44  double lastError = optimizer.error();
45  do {
46  gttic_(Iterate_optimizer);
47  optimizer.iterate();
48  gttoc_(Iterate_optimizer);
50  tictoc_print_();
51  cout << "Error: " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl;
52  break;
53  } while(!checkConvergence(optimizer.params().relativeErrorTol,
54  optimizer.params().absoluteErrorTol, optimizer.params().errorTol,
55  lastError, optimizer.error(), optimizer.params().verbosity));
56 
57  // Compute marginals
58  Marginals marginals(graph, optimizer.values());
59  int i=0;
60  for(Key key: initial.keys()) {
61  gttic_(marginalInformation);
63  gttoc_(marginalInformation);
65  if(i % 1000 == 0)
66  tictoc_print_();
67  ++i;
68  }
69 
70  } catch(std::exception& e) {
71  cout << e.what() << endl;
72  return 1;
73  }
74 
75  return 0;
76 }
#define gttic_(label)
Definition: timing.h:230
GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:500
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition: Half.h:150
NonlinearFactorGraph graph
KeyVector keys() const
Definition: Values.cpp:191
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
string findExampleDataFile(const string &name)
Definition: dataset.cpp:65
else if n * info
double errorTol
The maximum total error to stop iterating (default 0.0)
int main(int argc, char *argv[])
Definition: timeBatch.cpp:25
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
const Values & values() const
return values in current optimizer state
int data[]
const LevenbergMarquardtParams & params() const
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
void tictoc_print_()
Definition: timing.h:253
Array< double, 1, 3 > e(1./3., 0.5, 2.)
double lambda() const
Access the current damping value.
double error() const
return error in current optimizer state
traits
Definition: chartTesting.h:28
GaussianFactorGraph::shared_ptr iterate() override
A class for computing marginals in a NonlinearFactorGraph.
void tictoc_finishedIteration_()
Definition: timing.h:249
#define gttoc_(label)
Definition: timing.h:235
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
Marginals marginals(graph, result)
Timing utilities.
Matrix marginalInformation(Key variable) const
Definition: Marginals.cpp:121


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:50:29