Go to the documentation of this file.
2 GTSAM Copyright 2010, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 See LICENSE for the license information
8 A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
9 using LAGO (Linear Approximation for Graph Optimization).
10 Output is written to a file, in g2o format
13 L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
14 approximation for planar pose graph optimization, IJRR, 2014.
16 L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
17 for graph-based simultaneous localization and mapping, RSS, 2011.
19 Author: Luca Carlone (C++), John Lambert (Python)
23 from argparse
import Namespace
28 from gtsam
import Point3, Pose2, PriorFactorPose2, Values
31 def run(args: Namespace) ->
None:
32 """Run LAGO on input data stored in g2o file."""
43 print(
"Computing LAGO estimate")
47 if args.output
is None:
48 estimateLago.print(
"estimateLago")
50 outputFile = args.output
51 print(
"Writing results to file: ", outputFile)
58 if __name__ ==
"__main__":
59 parser = argparse.ArgumentParser(
60 description=
"A 2D Pose SLAM example that reads input from g2o, "
61 "converts it to a factor graph and does the optimization. "
62 "Output is written on a file, in g2o format"
64 parser.add_argument(
"-i",
"--input", help=
"input file g2o format")
65 parser.add_argument(
"-o",
"--output", help=
"the path to the output file with optimized graph")
66 args = parser.parse_args()
Values initialize(const NonlinearFactorGraph &graph, const Values &initialGuess)
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...
PriorFactor< Pose2 > PriorFactorPose2
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 Sat Nov 16 2024 04:03:36