Pose2SLAMExample_lago.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 
22 #include <gtsam/slam/lago.h>
23 #include <gtsam/slam/dataset.h>
24 #include <gtsam/geometry/Pose2.h>
25 #include <fstream>
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 int main(const int argc, const char *argv[]) {
31  // Read graph from file
32  string g2oFile;
33  if (argc < 2)
34  g2oFile = findExampleDataFile("noisyToyGraph.txt");
35  else
36  g2oFile = argv[1];
37 
39  Values::shared_ptr initial;
40  std::tie(graph, initial) = readG2o(g2oFile);
41 
42  // Add prior on the pose having index (key) = 0
43  auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
44  graph->addPrior(0, Pose2(), priorModel);
45  graph->print();
46 
47  std::cout << "Computing LAGO estimate" << std::endl;
48  Values estimateLago = lago::initialize(*graph);
49  std::cout << "done!" << std::endl;
50 
51  if (argc < 3) {
52  estimateLago.print("estimateLago");
53  } else {
54  const string outputFile = argv[2];
55  std::cout << "Writing results to file: " << outputFile << std::endl;
57  Values::shared_ptr initial2;
58  std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
59  writeG2o(*graphNoKernel, estimateLago, outputFile);
60  std::cout << "done! " << std::endl;
61  }
62 
63  return 0;
64 }
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
Eigen::Vector3d Vector3
Definition: Vector.h:43
Values initial
Definition: BFloat16.h:88
NonlinearFactorGraph graph
Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization). see papers:
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
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
int main(const int argc, const char *argv[])
2D Pose
Values initialize(const NonlinearFactorGraph &graph, bool useOdometricPath)
Definition: lago.cpp:375
utility functions for loading datasets


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