LocalizationExample.py
Go to the documentation of this file.
1 """
2 A simple 2D pose slam example with "GPS" measurements
3  - The robot moves forward 2 meter each iteration
4  - The robot initially faces along the X axis (horizontal, to the right in 2D)
5  - We have full odometry between pose
6  - We have "GPS-like" measurements implemented with a custom factor
7 """
8 import numpy as np
9 
10 import gtsam
11 from gtsam import BetweenFactorPose2, Pose2, noiseModel
12 from gtsam_unstable import PartialPriorFactorPose2
13 
14 
15 def main():
16  # 1. Create a factor graph container and add factors to it.
18 
19  # 2a. Add odometry factors
20  # For simplicity, we will use the same noise model for each odometry factor
21  odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1]))
22 
23  # Create odometry (Between) factors between consecutive poses
24  graph.push_back(
25  BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise))
26  graph.push_back(
27  BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise))
28 
29  # 2b. Add "GPS-like" measurements
30  # We will use PartialPrior factor for this.
31  unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1,
32  0.1])) # 10cm std on x,y
33 
34  graph.push_back(
35  PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise))
36  graph.push_back(
37  PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise))
38  graph.push_back(
39  PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise))
40  graph.print("\nFactor Graph:\n")
41 
42  # 3. Create the data structure to hold the initialEstimate estimate to the solution
43  # For illustrative purposes, these have been deliberately set to incorrect values
44  initialEstimate = gtsam.Values()
45  initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2))
46  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2))
47  initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1))
48  initialEstimate.print("\nInitial Estimate:\n")
49 
50  # 4. Optimize using Levenberg-Marquardt optimization. The optimizer
51  # accepts an optional set of configuration parameters, controlling
52  # things like convergence criteria, the type of linear system solver
53  # to use, and the amount of information displayed during optimization.
54  # Here we will use the default set of parameters. See the
55  # documentation for the full set of parameters.
56  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
57  result = optimizer.optimize()
58  result.print("Final Result:\n")
59 
60  # 5. Calculate and print marginal covariances for all variables
61  marginals = gtsam.Marginals(graph, result)
62  print("x1 covariance:\n", marginals.marginalCovariance(1))
63  print("x2 covariance:\n", marginals.marginalCovariance(2))
64  print("x3 covariance:\n", marginals.marginalCovariance(3))
65 
66 
67 if __name__ == "__main__":
68  main()
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
gtsam::Marginals
Definition: Marginals.h:32
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
BetweenFactorPose2
BetweenFactor< Pose2 > BetweenFactorPose2
Definition: serialization.cpp:49
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
gtsam_unstable.examples.LocalizationExample.main
def main()
Definition: LocalizationExample.py:15
gtsam::Values
Definition: Values.h:65
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:44