Pose2SLAMExample_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 
21 #include <gtsam/slam/dataset.h>
22 #include <gtsam/geometry/Pose2.h>
24 #include <fstream>
25 
26 using namespace std;
27 using namespace gtsam;
28 
29 // HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
30 int main(const int argc, const char *argv[]) {
31  string kernelType = "none";
32  int maxIterations = 100; // default
33  string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
34 
35  // Parse user's inputs
36  if (argc > 1) {
37  g2oFile = argv[1]; // input dataset filename
38  }
39  if (argc > 3) {
40  maxIterations = atoi(argv[3]); // user can specify either tukey or huber
41  }
42  if (argc > 4) {
43  kernelType = argv[4]; // user can specify either tukey or huber
44  }
45 
46  // reading file and creating factor graph
48  Values::shared_ptr initial;
49  bool is3D = false;
50  if (kernelType.compare("none") == 0) {
51  std::tie(graph, initial) = readG2o(g2oFile, is3D);
52  }
53  if (kernelType.compare("huber") == 0) {
54  std::cout << "Using robust kernel: huber " << std::endl;
55  std::tie(graph, initial) =
56  readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
57  }
58  if (kernelType.compare("tukey") == 0) {
59  std::cout << "Using robust kernel: tukey " << std::endl;
60  std::tie(graph, initial) =
61  readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
62  }
63 
64  // Add prior on the pose having index (key) = 0
65  auto priorModel = //
66  noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
67  graph->addPrior(0, Pose2(), priorModel);
68  std::cout << "Adding prior on pose 0 " << std::endl;
69 
71  params.setVerbosity("TERMINATION");
72  if (argc > 3) {
73  params.maxIterations = maxIterations;
74  std::cout << "User required to perform maximum " << params.maxIterations
75  << " iterations " << std::endl;
76  }
77 
78  std::cout << "Optimizing the factor graph" << std::endl;
80  Values result = optimizer.optimize();
81  std::cout << "Optimization complete" << std::endl;
82 
83  std::cout << "initial error=" << graph->error(*initial) << std::endl;
84  std::cout << "final error=" << graph->error(result) << std::endl;
85 
86  if (argc < 3) {
87  result.print("result");
88  } else {
89  const string outputFile = argv[2];
90  std::cout << "Writing results to file: " << outputFile << std::endl;
92  Values::shared_ptr initial2;
93  std::tie(graphNoKernel, initial2) = readG2o(g2oFile);
94  writeG2o(*graphNoKernel, result, outputFile);
95  std::cout << "done! " << std::endl;
96  }
97  return 0;
98 }
Pose2.h
2D Pose
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::readG2o
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
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
main
int main(const int argc, const char *argv[])
Definition: Pose2SLAMExample_g2o.cpp:30
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
result
Values result
Definition: OdometryOptimize.cpp:8
GaussNewtonOptimizer.h
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
Definition: NonlinearFactorGraph.cpp:170
dataset.h
utility functions for loading datasets
initial2
Definition: testScenarioRunner.cpp:209
gtsam::GaussNewtonParams
Definition: GaussNewtonOptimizer.h:30
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Definition: NonlinearFactorGraph.h:199
gtsam::KernelFunctionTypeHUBER
@ KernelFunctionTypeHUBER
Definition: dataset.h:75
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
gtsam::KernelFunctionTypeTUKEY
@ KernelFunctionTypeTUKEY
Definition: dataset.h:75
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
outputFile
string outputFile
Definition: SolverComparer.cpp:88
initial
Definition: testScenarioRunner.cpp:148
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Pose2
Definition: Pose2.h:39
gtsam::NonlinearFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactorGraph.h:61
gtsam::writeG2o
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


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24