2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 CustomFactor demo that simulates a 1-D sensor fusion task.
9 Author: Fan Jiang, Frank Dellaert
12 from functools
import partial
13 from typing
import List, Optional
22 """Simulate a car for one second"""
26 x = [x0 + v * dt * i
for i
in range(5)]
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
42 estimate = values.atVector(key)
43 error = estimate - measurement
44 if jacobians
is not None:
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
62 pos1, pos2 = values.atVector(key1), values.atVector(key2)
63 error = (pos2 - pos1) - measurement
64 if jacobians
is not None:
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
82 pos = values.atVector(key)
83 error = pos - measurement
84 if jacobians
is not None:
94 print(f
"Simulated car trajectory: {x}")
101 x[k] + (np.random.normal(scale=sigma_gps)
if add_noise
else 0)
109 (np.random.normal(scale=sigma_odo)
if add_noise
else 0)
118 z_0 = x[0] - lm_0 + (np.random.normal(scale=sigma_lm)
if add_noise
else 0)
122 z_3 = x[3] - lm_3 + (np.random.normal(scale=sigma_lm)
if add_noise
else 0)
126 print(
"unknowns = ",
list(map(gtsam.DefaultKeyFormatter, unknown)))
137 partial(error_gps, np.array([g[k]])))
145 v.insert(unknown[i], np.array([0.0]))
152 result = optimizer.optimize()
155 error = np.array([(result.atVector(unknown[k]) - x[k])[0]
158 print(
"Result with only GPS")
159 print(result, np.round(error, 2),
160 f
"\nJ(X)={0.5 * np.sum(np.square(error))}")
167 partial(error_odom, np.array([o[k]])))
168 factor_graph.add(odof)
173 result = optimizer.optimize()
175 error = np.array([(result.atVector(unknown[k]) - x[k])[0]
178 print(
"Result with GPS+Odometry")
179 print(result, np.round(error, 2),
180 f
"\nJ(X)={0.5 * np.sum(np.square(error))}")
187 partial(error_lm, np.array([lm_0 + z_0]))))
190 partial(error_lm, np.array([lm_3 + z_3]))))
195 result = optimizer.optimize()
197 error = np.array([(result.atVector(unknown[k]) - x[k])[0]
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))}")
205 if __name__ ==
"__main__":