TimeOfArrivalExample.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2020, 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 Track a moving object "Time of Arrival" measurements at 4 microphones.
10 Author: Frank Dellaert
11 """
12 # pylint: disable=invalid-name, no-name-in-module
13 
14 from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams,
15  NonlinearFactorGraph, Point3, Values, noiseModel)
16 from gtsam_unstable import Event, TimeOfArrival, TOAFactor
17 
18 # units
19 MS = 1e-3
20 CM = 1e-2
21 
22 # Instantiate functor with speed of sound value
23 TIME_OF_ARRIVAL = TimeOfArrival(330)
24 
25 
27  """Create microphones."""
28  height = 0.5
29  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))
34  return microphones
35 
36 
38  """Create ground truth trajectory."""
39  trajectory = []
40  timeOfEvent = 10
41  # simulate emitting a sound every second while moving on straight line
42  for key in range(n):
43  trajectory.append(
44  Event(timeOfEvent, 245 * CM + key * 1.0, 201.5 * CM, (212 - 45) * CM))
45  timeOfEvent += 1
46 
47  return trajectory
48 
49 
50 def simulate_one_toa(microphones, event):
51  """Simulate time-of-arrival measurements for a single event."""
52  return [TIME_OF_ARRIVAL.measure(event, microphones[i])
53  for i in range(len(microphones))]
54 
55 
56 def simulate_toa(microphones, trajectory):
57  """Simulate time-of-arrival measurements for an entire trajectory."""
58  return [simulate_one_toa(microphones, event)
59  for event in trajectory]
60 
61 
62 def create_graph(microphones, simulatedTOA):
63  """Create factor graph."""
64  graph = NonlinearFactorGraph()
65 
66  # Create a noise model for the TOA error
67  model = noiseModel.Isotropic.Sigma(1, 0.5 * MS)
68 
69  K = len(microphones)
70  key = 0
71  for toa in simulatedTOA:
72  for i in range(K):
73  factor = TOAFactor(key, microphones[i], toa[i], model)
74  graph.push_back(factor)
75  key += 1
76 
77  return graph
78 
79 
81  """Create initial estimate for n events."""
82  initial = Values()
83  zero = Event()
84  for key in range(n):
85  TOAFactor.InsertEvent(key, zero, initial)
86  return initial
87 
88 
90  """Run example with 4 microphones and 5 events in a straight line."""
91  # Create microphones
92  microphones = define_microphones()
93  K = len(microphones)
94  for i in range(K):
95  print("mic {} = {}".format(i, microphones[i]))
96 
97  # Create a ground truth trajectory
98  n = 5
99  groundTruth = create_trajectory(n)
100  for event in groundTruth:
101  print(event)
102 
103  # Simulate time-of-arrival measurements
104  simulatedTOA = simulate_toa(microphones, groundTruth)
105  for key in range(n):
106  for i in range(K):
107  print("z_{}{} = {} ms".format(key, i, simulatedTOA[key][i] / MS))
108 
109  # create factor graph
110  graph = create_graph(microphones, simulatedTOA)
111  print(graph.at(0))
112 
113  # Create initial estimate
114  initial_estimate = create_initial_estimate(n)
115  print(initial_estimate)
116 
117  # Optimize using Levenberg-Marquardt optimization.
118  params = LevenbergMarquardtParams()
119  params.setAbsoluteErrorTol(1e-10)
120  params.setVerbosityLM("SUMMARY")
121  optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params)
122  result = optimizer.optimize()
123  print("Final Result:\n", result)
124 
125 
126 if __name__ == '__main__':
127  toa_example()
128  print("Example complete")
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Double_ range(const Point2_ &p, const Point2_ &q)
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
Vector3 Point3
Definition: Point3.h:38
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2244


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:01