test_OdometryExample.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 Odometry unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 import unittest
12 
13 import numpy as np
14 
15 import gtsam
16 from gtsam.utils.test_case import GtsamTestCase
17 
18 
20 
22  # Create the graph (defined in pose2SLAM.h, derived from
23  # NonlinearFactorGraph)
25 
26  # Add a Gaussian prior on pose x_1
27  priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin
29  np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta
30  # add directly to graph
31  graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
32 
33  # Add two odometry factors
34  # create a measurement for both factors (the same in this case)
35  odometry = gtsam.Pose2(2.0, 0.0, 0.0)
36  odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
37  np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta
38  graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
39  graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
40 
41  # Initialize to noisy points
42  initialEstimate = gtsam.Values()
43  initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
44  initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
45  initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
46 
47  # Optimize using Levenberg-Marquardt optimization with an ordering from
48  # colamd
49  optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
50  result = optimizer.optimizeSafely()
51  marginals = gtsam.Marginals(graph, result)
52  marginals.marginalCovariance(1)
53 
54  # Check first pose equality
55  pose_1 = result.atPose2(1)
56  self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
57 
58 if __name__ == "__main__":
59  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