Go to the documentation of this file.
2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 See LICENSE for the license information
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
14 from __future__
import print_function
19 import matplotlib.pyplot
as plt
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')
34 help=
"the path to the output file with optimized graph")
35 parser.add_argument(
'-m',
38 help=
"maximum number of iterations for optimizer")
39 parser.add_argument(
'-k',
41 choices=[
'none',
'huber',
'tukey'],
43 help=
"Type of kernel used")
44 parser.add_argument(
"-p",
47 help=
"Flag to plot results")
48 args = parser.parse_args()
53 maxIterations = 100
if args.maxiter
is None else args.maxiter
59 assert args.kernel ==
"none",
"Supplied kernel type is not yet implemented"
63 graph.add(gtsam.PriorFactorPose2(0,
gtsam.Pose2(), priorModel))
66 params.setVerbosity(
"Termination")
67 params.setMaxIterations(maxIterations)
72 result = optimizer.optimize()
74 print(
"Optimization complete")
75 print(
"initial error = ", graph.error(initial))
76 print(
"final error = ", graph.error(result))
78 if args.output
is None:
83 outputFile = args.output
84 print(
"Writing results to file: ", outputFile)
91 for i
in range(resultPoses.shape[0]):
96 if __name__ ==
"__main__":
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
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...
Double_ range(const Point2_ &p, const Point2_ &q)
void print(const Matrix &A, const string &s, ostream &stream)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
static shared_ptr Variances(const Vector &variances, bool smart=true)
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24