OdometryExample.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 robot motion example, with prior and two odometry measurements
10 Author: Frank Dellaert
11 """
12 # pylint: disable=invalid-name, E1101
13 
14 from __future__ import print_function
15 
16 import gtsam
17 import gtsam.utils.plot as gtsam_plot
18 import matplotlib.pyplot as plt
19 import numpy as np
20 
21 # Create noise models
22 ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
23 PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
24 
25 
26 def main():
27  """Main runner"""
28  # Create an empty nonlinear factor graph
30 
31  # Add a prior on the first pose, setting it to the origin
32  # A prior factor consists of a mean and a noise model (covariance matrix)
33  priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
34  graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
35 
36  # Add odometry factors
37  odometry = gtsam.Pose2(2.0, 0.0, 0.0)
38  # For simplicity, we will use the same noise model for each odometry factor
39  # Create odometry (Between) factors between consecutive poses
40  graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
41  graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
42  print("\nFactor Graph:\n{}".format(graph))
43 
44  # Create the data structure to hold the initialEstimate estimate to the solution
45  # For illustrative purposes, these have been deliberately set to incorrect values
46  initial = gtsam.Values()
47  initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
48  initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
49  initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
50  print("\nInitial Estimate:\n{}".format(initial))
51 
52  # optimize using Levenberg-Marquardt optimization
54  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
55  result = optimizer.optimize()
56  print("\nFinal Result:\n{}".format(result))
57 
58  # 5. Calculate and print marginal covariances for all variables
59  marginals = gtsam.Marginals(graph, result)
60  for i in range(1, 4):
61  print("X{} covariance:\n{}\n".format(i,
62  marginals.marginalCovariance(i)))
63 
64  for i in range(1, 4):
65  gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
66  marginals.marginalCovariance(i))
67  plt.axis('equal')
68  plt.show()
69 
70 
71 if __name__ == "__main__":
72  main()
gtsam.examples.OdometryExample.main
def main()
Definition: OdometryExample.py:26
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::Marginals
Definition: Marginals.h:32
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam::Values
Definition: Values.h:65
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::utils.plot
Definition: plot.py:1
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:52