2 GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
5 Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 See LICENSE for the license information
9 Track a moving object "Time of Arrival" measurements at 4 microphones.
10 Author: Frank Dellaert
14 from gtsam
import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
15 NonlinearFactorGraph, Point3, Values, noiseModel)
16 from gtsam_unstable
import Event, TimeOfArrival, TOAFactor
23 TIME_OF_ARRIVAL = TimeOfArrival(330)
27 """Create microphones."""
30 microphones.append(
Point3(0, 0, height))
31 microphones.append(
Point3(403 * CM, 0, height))
32 microphones.append(
Point3(403 * CM, 403 * CM, height))
33 microphones.append(
Point3(0, 403 * CM, 2 * height))
38 """Create ground truth trajectory."""
44 Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
51 """Simulate time-of-arrival measurements for a single event."""
52 return [TIME_OF_ARRIVAL.measure(event, microphones[i])
57 """Simulate time-of-arrival measurements for an entire trajectory."""
59 for event
in trajectory]
63 """Create factor graph."""
67 model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)
71 for toa
in simulatedTOA:
73 factor = TOAFactor(key, microphones[i], toa[i], model)
74 graph.push_back(factor)
81 """Create initial estimate for n events."""
85 TOAFactor.InsertEvent(key, zero, initial)
90 """Run example with 4 microphones and 5 events in a straight line."""
100 for event
in groundTruth:
107 print(
"z_{}{} = {} ms".
format(key, i, simulatedTOA[key][i] / MS))
115 print(initial_estimate)
119 params.setAbsoluteErrorTol(1e-10)
120 params.setVerbosityLM(
"SUMMARY")
122 result = optimizer.optimize()
123 print(
"Final Result:\n", result)
126 if __name__ ==
'__main__':
128 print(
"Example complete")