test_Pose3SLAMExample.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 PoseSLAM 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 from gtsam.utils.circlePose3 import circlePose3
19 
20 
22 
23  def test_Pose3SLAMExample(self) -> None:
24  # Create a hexagon of poses
25  hexagon = circlePose3(numPoses=6, radius=1.0)
26  p0 = hexagon.atPose3(0)
27  p1 = hexagon.atPose3(1)
28 
29  # create a Pose graph with one equality constraint and one measurement
31  fg.add(gtsam.NonlinearEqualityPose3(0, p0))
32  delta = p0.between(p1)
34  np.array([0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.)]))
35  fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
36  fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
37  fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))
38  fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance))
39  fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance))
40  fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance))
41 
42  # Create initial config
43  initial = gtsam.Values()
44  s = 0.10
45  initial.insert(0, p0)
46  initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1)))
47  initial.insert(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1)))
48  initial.insert(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1)))
49  initial.insert(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1)))
50  initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
51 
52  # optimize
53  optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial)
54  result = optimizer.optimizeSafely()
55 
56  pose_1 = result.atPose3(1)
57  self.gtsamAssertEquals(pose_1, p1, 1e-4)
58 
59 if __name__ == "__main__":
60  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