PlanarSLAMExample.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 Simple robotics example using odometry measurements and bearing-range (laser) measurements
10 Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)
11 """
12 # pylint: disable=invalid-name, E1101
13 
14 from __future__ import print_function
15 
16 import gtsam
17 import numpy as np
18 from gtsam.symbol_shorthand import L, X
19 
20 # Create noise models
21 PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
22 ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
23 MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))
24 
25 
26 def main():
27  """Main runner"""
28 
29  # Create an empty nonlinear factor graph
31 
32  # Create the keys corresponding to unknown variables in the factor graph
33  X1 = X(1)
34  X2 = X(2)
35  X3 = X(3)
36  L1 = L(4)
37  L2 = L(5)
38 
39  # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
40  graph.add(
41  gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
42 
43  # Add odometry factors between X1,X2 and X2,X3, respectively
44  graph.add(
45  gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
46  ODOMETRY_NOISE))
47  graph.add(
48  gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
49  ODOMETRY_NOISE))
50 
51  # Add Range-Bearing measurements to two different landmarks L1 and L2
52  graph.add(
53  gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
54  np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
55  graph.add(
56  gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
57  MEASUREMENT_NOISE))
58  graph.add(
59  gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
60  MEASUREMENT_NOISE))
61 
62  # Print graph
63  print("Factor Graph:\n{}".format(graph))
64 
65  # Create (deliberately inaccurate) initial estimate
66  initial_estimate = gtsam.Values()
67  initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
68  initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
69  initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
70  initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
71  initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
72 
73  # Print
74  print("Initial Estimate:\n{}".format(initial_estimate))
75 
76  # Optimize using Levenberg-Marquardt optimization. The optimizer
77  # accepts an optional set of configuration parameters, controlling
78  # things like convergence criteria, the type of linear system solver
79  # to use, and the amount of information displayed during optimization.
80  # Here we will use the default set of parameters. See the
81  # documentation for the full set of parameters.
83  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
84  params)
85  result = optimizer.optimize()
86  print("\nFinal Result:\n{}".format(result))
87 
88  # Calculate and print marginal covariances for all variables
89  marginals = gtsam.Marginals(graph, result)
90  for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
91  (L2, "L2")]:
92  print("{} covariance:\n{}\n".format(s,
93  marginals.marginalCovariance(key)))
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::Rot2::fromDegrees
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition: Rot2.h:66
gtsam::Marginals
Definition: Marginals.h:32
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
X
#define X
Definition: icosphere.cpp:20
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam.examples.PlanarSLAMExample.main
def main()
Definition: PlanarSLAMExample.py:26
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
L
MatrixXd L
Definition: LLT_example.cpp:6
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Values
Definition: Values.h:65
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:23