Pose3SLAMExample_g2o.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  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/slam/dataset.h>
23 #include <fstream>
24 
25 using namespace std;
26 using namespace gtsam;
27 
28 int main(const int argc, const char* argv[]) {
29  // Read graph from file
30  string g2oFile;
31  if (argc < 2)
32  g2oFile = findExampleDataFile("pose3example.txt");
33  else
34  g2oFile = argv[1];
35 
37  Values::shared_ptr initial;
38  bool is3D = true;
39  std::tie(graph, initial) = readG2o(g2oFile, is3D);
40 
41  // Add prior on the first key
42  auto priorModel = noiseModel::Diagonal::Variances(
43  (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
44  Key firstKey = 0;
45  for (const auto key : initial->keys()) {
46  std::cout << "Adding prior to g2o file " << std::endl;
47  firstKey = key;
48  graph->addPrior(firstKey, Pose3(), priorModel);
49  break;
50  }
51 
52  std::cout << "Optimizing the factor graph" << std::endl;
54  params.setVerbosity("TERMINATION"); // show info about stopping conditions
55  GaussNewtonOptimizer optimizer(*graph, *initial, params);
56  Values result = optimizer.optimize();
57  std::cout << "Optimization complete" << std::endl;
58 
59  std::cout << "initial error=" << graph->error(*initial) << std::endl;
60  std::cout << "final error=" << graph->error(result) << std::endl;
61 
62  if (argc < 3) {
63  result.print("result");
64  } else {
65  const string outputFile = argv[2];
66  std::cout << "Writing results to file: " << outputFile << std::endl;
68  Values::shared_ptr initial2;
69  std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
70  writeG2o(*graphNoKernel, result, outputFile);
71  std::cout << "done! " << std::endl;
72  }
73  return 0;
74 }
const gtsam::Symbol key('X', 0)
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure. ...
Definition: dataset.cpp:636
virtual const Values & optimize()
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
int main(const int argc, const char *argv[])
static const SmartProjectionParams params
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
Values result
void setVerbosity(const std::string &src)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:621
string outputFile
traits
Definition: chartTesting.h:28
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
std::shared_ptr< This > shared_ptr
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:15