test_Pose2SLAMExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 Pose2SLAM unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 import unittest
12 from math import pi
13 
14 import numpy as np
15 
16 import gtsam
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 
21 
23  # Assumptions
24  # - All values are axis aligned
25  # - Robot poses are facing along the X axis (horizontal, to the right in images)
26  # - We have full odometry for measurements
27  # - The robot is on a grid, moving 2 meters each step
28 
29  # Create graph container and add factors to it
31 
32  # Add prior
33  # gaussian for prior
34  priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
35  priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
36  # add directly to graph
37  graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
38 
39  # Add odometry
40  # general noisemodel for odometry
41  odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
42  graph.add(gtsam.BetweenFactorPose2(
43  1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise))
44  graph.add(gtsam.BetweenFactorPose2(
45  2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
46  graph.add(gtsam.BetweenFactorPose2(
47  3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
48  graph.add(gtsam.BetweenFactorPose2(
49  4, 5, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise))
50 
51  # Add pose constraint
52  model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
53  graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model))
54 
55  # Initialize to noisy points
56  initialEstimate = gtsam.Values()
57  initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
58  initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
59  initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
60  initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
61  initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
62 
63  # Optimize using Levenberg-Marquardt optimization with an ordering from
64  # colamd
65  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
66  result = optimizer.optimizeSafely()
67 
68  # Plot Covariance Ellipses
69  marginals = gtsam.Marginals(graph, result)
70  P = marginals.marginalCovariance(1)
71 
72  pose_1 = result.atPose2(1)
73  self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
74 
75 if __name__ == "__main__":
76  unittest.main()
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:46