Pose2SLAMExample_lago.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010, 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 See LICENSE for the license information
7 
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
11 
12 Reference:
13 L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
14 approximation for planar pose graph optimization, IJRR, 2014.
15 
16 L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
17 for graph-based simultaneous localization and mapping, RSS, 2011.
18 
19 Author: Luca Carlone (C++), John Lambert (Python)
20 """
21 
22 import argparse
23 from argparse import Namespace
24 
25 import numpy as np
26 
27 import gtsam
28 from gtsam import Point3, Pose2, PriorFactorPose2, Values
29 
30 
31 def run(args: Namespace) -> None:
32  """Run LAGO on input data stored in g2o file."""
33  g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None else args.input
34 
36  graph, initial = gtsam.readG2o(g2oFile)
37 
38  # Add prior on the pose having index (key) = 0
39  priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8))
40  graph.add(PriorFactorPose2(0, Pose2(), priorModel))
41  print(graph)
42 
43  print("Computing LAGO estimate")
44  estimateLago: Values = gtsam.lago.initialize(graph)
45  print("done!")
46 
47  if args.output is None:
48  estimateLago.print("estimateLago")
49  else:
50  outputFile = args.output
51  print("Writing results to file: ", outputFile)
52  graphNoKernel = gtsam.NonlinearFactorGraph()
53  graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
54  gtsam.writeG2o(graphNoKernel, estimateLago, outputFile)
55  print("Done! ")
56 
57 
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"
63  )
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()
67  run(args)
gtsam::lago::initialize
Values initialize(const NonlinearFactorGraph &graph, const Values &initialGuess)
Definition: lago.cpp:391
gtsam.examples.Pose2SLAMExample_lago.run
None run(Namespace args)
Definition: Pose2SLAMExample_lago.py:31
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
PriorFactorPose2
PriorFactor< Pose2 > PriorFactorPose2
Definition: serialization.cpp:37
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
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