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 import argparse
16 import math
17 import numpy as np
18 import matplotlib.pyplot as plt
19 
20 import gtsam
21 from gtsam.utils import plot
22 
23 
24 def vector3(x, y, z):
25  """Create 3d double numpy array."""
26  return np.array([x, y, z], dtype=float)
27 
28 
29 parser = argparse.ArgumentParser(
30  description="A 2D Pose SLAM example that reads input from g2o, "
31  "converts it to a factor graph and does the optimization. "
32  "Output is written on a file, in g2o format")
33 parser.add_argument('-i', '--input', help='input file g2o format')
34 parser.add_argument('-o', '--output',
35  help="the path to the output file with optimized graph")
36 parser.add_argument('-m', '--maxiter', type=int,
37  help="maximum number of iterations for optimizer")
38 parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'],
39  default="none", help="Type of kernel used")
40 parser.add_argument("-p", "--plot", action="store_true",
41  help="Flag to plot results")
42 args = parser.parse_args()
43 
44 g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
45  else args.input
46 
47 maxIterations = 100 if args.maxiter is None else args.maxiter
48 
49 is3D = False
50 
51 graph, initial = gtsam.readG2o(g2oFile, is3D)
52 
53 assert args.kernel == "none", "Supplied kernel type is not yet implemented"
54 
55 # Add prior on the pose having index (key) = 0
56 priorModel = gtsam.noiseModel.Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
57 graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
58 
60 params.setVerbosity("Termination")
61 params.setMaxIterations(maxIterations)
62 # parameters.setRelativeErrorTol(1e-5)
63 # Create the optimizer ...
64 optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
65 # ... and optimize
66 result = optimizer.optimize()
67 
68 print("Optimization complete")
69 print("initial error = ", graph.error(initial))
70 print("final error = ", graph.error(result))
71 
72 
73 if args.output is None:
74  print("\nFactor Graph:\n{}".format(graph))
75  print("\nInitial Estimate:\n{}".format(initial))
76  print("Final Result:\n{}".format(result))
77 else:
78  outputFile = args.output
79  print("Writing results to file: ", outputFile)
80  graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
81  gtsam.writeG2o(graphNoKernel, result, outputFile)
82  print ("Done!")
83 
84 if args.plot:
85  resultPoses = gtsam.utilities.extractPose2(result)
86  for i in range(resultPoses.shape[0]):
87  plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
88  plt.show()
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
GTSAM_EXPORT GraphAndValues readG2o(const std::string &g2oFile, const bool is3D=false, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:615
GTSAM_EXPORT 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:630
static shared_ptr Variances(const Vector &variances, bool smart=true)
Definition: NoiseModel.cpp:258
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:65


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:27