CustomFactorExample.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 CustomFactor demo that simulates a 1-D sensor fusion task.
9 Author: Fan Jiang, Frank Dellaert
10 """
11 
12 from functools import partial
13 from typing import List, Optional
14 
15 import gtsam
16 import numpy as np
17 
18 I = np.eye(1) # Creates a 1-element, 2D array
19 
20 
21 def simulate_car() -> List[float]:
22  """Simulate a car for one second"""
23  x0 = 0
24  dt = 0.25 # 4 Hz, typical GPS
25  v = 144 * 1000 / 3600 # 144 km/hour = 90mph, pretty fast
26  x = [x0 + v * dt * i for i in range(5)]
27 
28  return x
29 
30 
31 def error_gps(measurement: np.ndarray, this: gtsam.CustomFactor,
32  values: gtsam.Values,
33  jacobians: Optional[List[np.ndarray]]) -> np.ndarray:
34  """GPS Factor error function
35  :param measurement: GPS measurement, to be filled with `partial`
36  :param this: gtsam.CustomFactor handle
37  :param values: gtsam.Values
38  :param jacobians: Optional list of Jacobians
39  :return: the unwhitened error
40  """
41  key = this.keys()[0]
42  estimate = values.atVector(key)
43  error = estimate - measurement
44  if jacobians is not None:
45  jacobians[0] = I
46 
47  return error # with input types this is a 1D np.ndarray
48 
49 
50 def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor,
51  values: gtsam.Values,
52  jacobians: Optional[List[np.ndarray]]) -> np.ndarray:
53  """Odometry Factor error function
54  :param measurement: Odometry measurement, to be filled with `partial`
55  :param this: gtsam.CustomFactor handle
56  :param values: gtsam.Values
57  :param jacobians: Optional list of Jacobians
58  :return: the unwhitened error
59  """
60  key1 = this.keys()[0]
61  key2 = this.keys()[1]
62  pos1, pos2 = values.atVector(key1), values.atVector(key2)
63  error = (pos2 - pos1) - measurement
64  if jacobians is not None:
65  jacobians[0] = -I
66  jacobians[1] = I
67 
68  return error
69 
70 
71 def error_lm(measurement: np.ndarray, this: gtsam.CustomFactor,
72  values: gtsam.Values,
73  jacobians: Optional[List[np.ndarray]]) -> np.ndarray:
74  """Landmark Factor error function
75  :param measurement: Landmark measurement, to be filled with `partial`
76  :param this: gtsam.CustomFactor handle
77  :param values: gtsam.Values
78  :param jacobians: Optional list of Jacobians
79  :return: the unwhitened error
80  """
81  key = this.keys()[0]
82  pos = values.atVector(key)
83  error = pos - measurement
84  if jacobians is not None:
85  jacobians[0] = I
86 
87  return error
88 
89 
90 def main():
91  """Main runner."""
92 
93  x = simulate_car()
94  print(f"Simulated car trajectory: {x}")
95 
96  add_noise = True # set this to False to run with "perfect" measurements
97 
98  # GPS measurements
99  sigma_gps = 3.0 # assume GPS is +/- 3m
100  g = [
101  x[k] + (np.random.normal(scale=sigma_gps) if add_noise else 0)
102  for k in range(5)
103  ]
104 
105  # Odometry measurements
106  sigma_odo = 0.1 # assume Odometry is 10cm accurate at 4Hz
107  o = [
108  x[k + 1] - x[k] +
109  (np.random.normal(scale=sigma_odo) if add_noise else 0)
110  for k in range(4)
111  ]
112 
113  # Landmark measurements:
114  sigma_lm = 1 # assume landmark measurement is accurate up to 1m
115 
116  # Assume first landmark is at x=5, we measure it at time k=0
117  lm_0 = 5.0
118  z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
119 
120  # Assume other landmark is at x=28, we measure it at time k=3
121  lm_3 = 28.0
122  z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm) if add_noise else 0)
123 
124  unknown = [gtsam.symbol('x', k) for k in range(5)]
125 
126  print("unknowns = ", list(map(gtsam.DefaultKeyFormatter, unknown)))
127 
128  # We now can use nonlinear factor graphs
129  factor_graph = gtsam.NonlinearFactorGraph()
130 
131  # Add factors for GPS measurements
132  gps_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_gps)
133 
134  # Add the GPS factors
135  for k in range(5):
136  gf = gtsam.CustomFactor(gps_model, [unknown[k]],
137  partial(error_gps, np.array([g[k]])))
138  factor_graph.add(gf)
139 
140  # New Values container
141  v = gtsam.Values()
142 
143  # Add initial estimates to the Values container
144  for i in range(5):
145  v.insert(unknown[i], np.array([0.0]))
146 
147  # Initialize optimizer
148  params = gtsam.GaussNewtonParams()
149  optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
150 
151  # Optimize the factor graph
152  result = optimizer.optimize()
153 
154  # calculate the error from ground truth
155  error = np.array([(result.atVector(unknown[k]) - x[k])[0]
156  for k in range(5)])
157 
158  print("Result with only GPS")
159  print(result, np.round(error, 2),
160  f"\nJ(X)={0.5 * np.sum(np.square(error))}")
161 
162  # Adding odometry will improve things a lot
163  odo_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_odo)
164 
165  for k in range(4):
166  odof = gtsam.CustomFactor(odo_model, [unknown[k], unknown[k + 1]],
167  partial(error_odom, np.array([o[k]])))
168  factor_graph.add(odof)
169 
170  params = gtsam.GaussNewtonParams()
171  optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
172 
173  result = optimizer.optimize()
174 
175  error = np.array([(result.atVector(unknown[k]) - x[k])[0]
176  for k in range(5)])
177 
178  print("Result with GPS+Odometry")
179  print(result, np.round(error, 2),
180  f"\nJ(X)={0.5 * np.sum(np.square(error))}")
181 
182  # This is great, but GPS noise is still apparent, so now we add the two landmarks
183  lm_model = gtsam.noiseModel.Isotropic.Sigma(1, sigma_lm)
184 
185  factor_graph.add(
186  gtsam.CustomFactor(lm_model, [unknown[0]],
187  partial(error_lm, np.array([lm_0 + z_0]))))
188  factor_graph.add(
189  gtsam.CustomFactor(lm_model, [unknown[3]],
190  partial(error_lm, np.array([lm_3 + z_3]))))
191 
192  params = gtsam.GaussNewtonParams()
193  optimizer = gtsam.GaussNewtonOptimizer(factor_graph, v, params)
194 
195  result = optimizer.optimize()
196 
197  error = np.array([(result.atVector(unknown[k]) - x[k])[0]
198  for k in range(5)])
199 
200  print("Result with GPS+Odometry+Landmark")
201  print(result, np.round(error, 2),
202  f"\nJ(X)={0.5 * np.sum(np.square(error))}")
203 
204 
205 if __name__ == "__main__":
206  main()
gtsam::GaussNewtonOptimizer
Definition: GaussNewtonOptimizer.h:38
list
Definition: pytypes.h:2166
gtsam.examples.CustomFactorExample.simulate_car
List[float] simulate_car()
Definition: CustomFactorExample.py:21
gtsam::CustomFactor
Definition: CustomFactor.h:45
gtsam.examples.CustomFactorExample.error_gps
np.ndarray error_gps(np.ndarray measurement, gtsam.CustomFactor this, gtsam.Values values, Optional[List[np.ndarray]] jacobians)
Definition: CustomFactorExample.py:31
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam.examples.CustomFactorExample.main
def main()
Definition: CustomFactorExample.py:90
gtsam::GaussNewtonParams
Definition: GaussNewtonOptimizer.h:30
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam.examples.CustomFactorExample.error_lm
np.ndarray error_lm(np.ndarray measurement, gtsam.CustomFactor this, gtsam.Values values, Optional[List[np.ndarray]] jacobians)
Definition: CustomFactorExample.py:71
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:139
gtsam::Values
Definition: Values.h:65
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
gtsam.examples.CustomFactorExample.error_odom
np.ndarray error_odom(np.ndarray measurement, gtsam.CustomFactor this, gtsam.Values values, Optional[List[np.ndarray]] jacobians)
Definition: CustomFactorExample.py:50


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