Pose2SLAMExample_g2o.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7 See LICENSE for the license information
8 
9 A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph
10 and does the optimization. Output is written on a file, in g2o format
11 """
12 # pylint: disable=invalid-name, E1101
13 
14 from __future__ import print_function
15 
16 import argparse
17 
18 import gtsam
19 import matplotlib.pyplot as plt
20 from gtsam.utils import plot
21 
22 
23 def main():
24  """Main runner."""
25 
26  parser = argparse.ArgumentParser(
27  description="A 2D Pose SLAM example that reads input from g2o, "
28  "converts it to a factor graph and does the optimization. "
29  "Output is written on a file, in g2o format")
30  parser.add_argument('-i', '--input', help='input file g2o format')
31  parser.add_argument(
32  '-o',
33  '--output',
34  help="the path to the output file with optimized graph")
35  parser.add_argument('-m',
36  '--maxiter',
37  type=int,
38  help="maximum number of iterations for optimizer")
39  parser.add_argument('-k',
40  '--kernel',
41  choices=['none', 'huber', 'tukey'],
42  default="none",
43  help="Type of kernel used")
44  parser.add_argument("-p",
45  "--plot",
46  action="store_true",
47  help="Flag to plot results")
48  args = parser.parse_args()
49 
50  g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
51  else args.input
52 
53  maxIterations = 100 if args.maxiter is None else args.maxiter
54 
55  is3D = False
56 
57  graph, initial = gtsam.readG2o(g2oFile, is3D)
58 
59  assert args.kernel == "none", "Supplied kernel type is not yet implemented"
60 
61  # Add prior on the pose having index (key) = 0
62  priorModel = gtsam.noiseModel.Diagonal.Variances(gtsam.Point3(1e-6, 1e-6, 1e-8))
63  graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
64 
65  params = gtsam.GaussNewtonParams()
66  params.setVerbosity("Termination")
67  params.setMaxIterations(maxIterations)
68  # parameters.setRelativeErrorTol(1e-5)
69  # Create the optimizer ...
70  optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
71  # ... and optimize
72  result = optimizer.optimize()
73 
74  print("Optimization complete")
75  print("initial error = ", graph.error(initial))
76  print("final error = ", graph.error(result))
77 
78  if args.output is None:
79  print("\nFactor Graph:\n{}".format(graph))
80  print("\nInitial Estimate:\n{}".format(initial))
81  print("Final Result:\n{}".format(result))
82  else:
83  outputFile = args.output
84  print("Writing results to file: ", outputFile)
85  graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
86  gtsam.writeG2o(graphNoKernel, result, outputFile)
87  print("Done!")
88 
89  if args.plot:
90  resultPoses = gtsam.utilities.extractPose2(result)
91  for i in range(resultPoses.shape[0]):
92  plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
93  plt.show()
94 
95 
96 if __name__ == "__main__":
97  main()
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
Definition: openglsupport.cpp:226
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
gtsam::utilities::extractPose2
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
Definition: nonlinear/utilities.h:140
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::utils
Definition: kruskal-inl.h:31
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
gtsam.examples.Pose2SLAMExample_g2o.main
def main()
Definition: Pose2SLAMExample_g2o.py:23
gtsam::GaussNewtonParams
Definition: GaussNewtonOptimizer.h:30
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam::noiseModel::Diagonal::Variances
static shared_ptr Variances(const Vector &variances, bool smart=true)
Definition: NoiseModel.cpp:284
gtsam::Pose2
Definition: Pose2.h:39
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 Sun Dec 22 2024 04:12:45