test_FixedLagSmootherExample.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 Cal3Unified unit tests.
9 Author: Frank Dellaert & Duy Nguyen Ta (Python)
10 """
11 import unittest
12 
13 import numpy as np
14 from gtsam.utils.test_case import GtsamTestCase
15 
16 import gtsam
17 
18 
20  '''
21  Tests the fixed lag smoother wrapper
22  '''
23 
25  '''
26  Simple test that checks for equality between C++ example
27  file and the Python implementation. See
28  gtsam_unstable/examples/FixedLagSmootherExample.cpp
29  '''
30  # Define a batch fixed lag smoother, which uses
31  # Levenberg-Marquardt to perform the nonlinear optimization
32  lag = 2.0
33  smoother_batch = gtsam.BatchFixedLagSmoother(lag)
34 
35  # Create containers to store the factors and linearization points
36  # that will be sent to the smoothers
37  new_factors = gtsam.NonlinearFactorGraph()
38  new_values = gtsam.Values()
39  new_timestamps = {}
40 
41  # Create a prior on the first pose, placing it at the origin
42  prior_mean = gtsam.Pose2(0, 0, 0)
44  np.array([0.3, 0.3, 0.1]))
45  X1 = 0
46  new_factors.push_back(
47  gtsam.PriorFactorPose2(
48  X1, prior_mean, prior_noise))
49  new_values.insert(X1, prior_mean)
50  new_timestamps[X1] = 0.0
51 
52  delta_time = 0.25
53  time = 0.25
54 
55  i = 0
56 
57  ground_truth = [
58  gtsam.Pose2(0.995821, 0.0231012, 0.0300001),
59  gtsam.Pose2(1.49284, 0.0457247, 0.045),
60  gtsam.Pose2(1.98981, 0.0758879, 0.06),
61  gtsam.Pose2(2.48627, 0.113502, 0.075),
62  gtsam.Pose2(2.98211, 0.158558, 0.09),
63  gtsam.Pose2(3.47722, 0.211047, 0.105),
64  gtsam.Pose2(3.97149, 0.270956, 0.12),
65  gtsam.Pose2(4.4648, 0.338272, 0.135),
66  gtsam.Pose2(4.95705, 0.41298, 0.15),
67  gtsam.Pose2(5.44812, 0.495063, 0.165),
68  gtsam.Pose2(5.9379, 0.584503, 0.18),
69  ]
70 
71  # Iterates from 0.25s to 3.0s, adding 0.25s each loop
72  # In each iteration, the agent moves at a constant speed
73  # and its two odometers measure the change. The smoothed
74  # result is then compared to the ground truth
75  while time <= 3.0:
76  previous_key = int(1000 * (time - delta_time))
77  current_key = int(1000 * time)
78 
79  # assign current key to the current timestamp
80  new_timestamps[current_key] = time
81 
82  # Add a guess for this pose to the new values
83  # Assume that the robot moves at 2 m/s. Position is time[s] *
84  # 2[m/s]
85  current_pose = gtsam.Pose2(time * 2, 0, 0)
86  new_values.insert(current_key, current_pose)
87 
88  # Add odometry factors from two different sources with different
89  # error stats
90  odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
91  odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
92  np.array([0.1, 0.1, 0.05]))
93  new_factors.push_back(
94  gtsam.BetweenFactorPose2(
95  previous_key,
96  current_key,
97  odometry_measurement_1,
98  odometry_noise_1))
99 
100  odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
101  odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
102  np.array([0.05, 0.05, 0.05]))
103  new_factors.push_back(
104  gtsam.BetweenFactorPose2(
105  previous_key,
106  current_key,
107  odometry_measurement_2,
108  odometry_noise_2))
109 
110  # Update the smoothers with the new factors. In this case,
111  # one iteration must pass for Levenberg-Marquardt to accurately
112  # estimate
113  if time >= 0.50:
114  smoother_batch.update(new_factors, new_values, new_timestamps)
115 
116  estimate = smoother_batch.calculateEstimatePose2(current_key)
117  self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
118  i += 1
119 
120  new_timestamps.clear()
121  new_values.clear()
122  new_factors.resize(0)
123 
124  time += delta_time
125 
126 
127 if __name__ == "__main__":
128  unittest.main()
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
test_FixedLagSmootherExample.TestFixedLagSmootherExample.test_FixedLagSmootherExample
def test_FixedLagSmootherExample(self)
Definition: test_FixedLagSmootherExample.py:24
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
test_FixedLagSmootherExample.TestFixedLagSmootherExample
Definition: test_FixedLagSmootherExample.py:19
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::BatchFixedLagSmoother
Definition: nonlinear/BatchFixedLagSmoother.h:29
gtsam::utils.test_case
Definition: test_case.py:1
gtsam::Values
Definition: Values.h:65
gtsam::utils.test_case.GtsamTestCase
Definition: test_case.py:15
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:27