FixedLagSmootherExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6 
7 See LICENSE for the license information
8 
9 Demonstration of the fixed-lag smoothers using a planar robot example
10 and multiple odometry-like sensors
11 Author: Frank Dellaert (C++), Jeremy Aguilon (Python)
12 """
13 
14 import numpy as np
15 
16 import gtsam
17 import gtsam_unstable
18 
19 
21  """
22  Runs a batch fixed smoother on an agent with two odometry
23  sensors that is simply moving to the
24  """
25 
26  # Define a batch fixed lag smoother, which uses
27  # Levenberg-Marquardt to perform the nonlinear optimization
28  lag = 2.0
29  smoother_batch = gtsam.BatchFixedLagSmoother(lag)
30 
31  # Create containers to store the factors and linearization points
32  # that will be sent to the smoothers
33  new_factors = gtsam.NonlinearFactorGraph()
34  new_values = gtsam.Values()
35  new_timestamps = {}
36 
37  # Create a prior on the first pose, placing it at the origin
38  prior_mean = gtsam.Pose2(0, 0, 0)
39  prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
40  X1 = 0
41  new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
42  new_values.insert(X1, prior_mean)
43  new_timestamps[X1] = 0.0
44 
45  delta_time = 0.25
46  time = 0.25
47 
48  while time <= 3.0:
49  previous_key = int(1000 * (time - delta_time))
50  current_key = int(1000 * time)
51 
52  # assign current key to the current timestamp
53  new_timestamps[current_key] = time
54 
55  # Add a guess for this pose to the new values
56  # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
57  current_pose = gtsam.Pose2(time * 2, 0, 0)
58  new_values.insert(current_key, current_pose)
59 
60  # Add odometry factors from two different sources with different error
61  # stats
62  odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
63  odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
64  np.array([0.1, 0.1, 0.05]))
65  new_factors.push_back(gtsam.BetweenFactorPose2(
66  previous_key, current_key, odometry_measurement_1, odometry_noise_1
67  ))
68 
69  odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
70  odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
71  np.array([0.05, 0.05, 0.05]))
72  new_factors.push_back(gtsam.BetweenFactorPose2(
73  previous_key, current_key, odometry_measurement_2, odometry_noise_2
74  ))
75 
76  # Update the smoothers with the new factors. In this case,
77  # one iteration must pass for Levenberg-Marquardt to accurately
78  # estimate
79  if time >= 0.50:
80  smoother_batch.update(new_factors, new_values, new_timestamps)
81  print("Timestamp = " + str(time) + ", Key = " + str(current_key))
82  print(smoother_batch.calculateEstimatePose2(current_key))
83 
84  new_timestamps.clear()
85  new_values.clear()
86  new_factors.resize(0)
87 
88  time += delta_time
89 
90 
91 if __name__ == '__main__':
93  print("Example complete")
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam.examples.FixedLagSmootherExample.BatchFixedLagSmootherExample
def BatchFixedLagSmootherExample()
Definition: FixedLagSmootherExample.py:20
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::BatchFixedLagSmoother
Definition: nonlinear/BatchFixedLagSmoother.h:29
str
Definition: pytypes.h:1558
gtsam::Values
Definition: Values.h:65
gtsam::Pose2
Definition: Pose2.h:39


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