TimeOfArrivalExample.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
26 
27 #include <boost/bind.hpp>
28 #include <boost/format.hpp>
29 
30 #include <vector>
31 
32 using namespace std;
33 using namespace gtsam;
34 
35 // units
36 static const double ms = 1e-3;
37 static const double cm = 1e-2;
38 
39 // Instantiate functor with speed of sound value
40 static const TimeOfArrival kTimeOfArrival(330);
41 
42 /* ************************************************************************* */
43 // Create microphones
44 vector<Point3> defineMicrophones() {
45  const double height = 0.5;
46  vector<Point3> microphones;
47  microphones.push_back(Point3(0, 0, height));
48  microphones.push_back(Point3(403 * cm, 0, height));
49  microphones.push_back(Point3(403 * cm, 403 * cm, height));
50  microphones.push_back(Point3(0, 403 * cm, 2 * height));
51  return microphones;
52 }
53 
54 /* ************************************************************************* */
55 // Create ground truth trajectory
56 vector<Event> createTrajectory(size_t n) {
57  vector<Event> trajectory;
58  double timeOfEvent = 10;
59  // simulate emitting a sound every second while moving on straight line
60  for (size_t key = 0; key < n; key++) {
61  trajectory.push_back(
62  Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm));
63  timeOfEvent += 1;
64  }
65  return trajectory;
66 }
67 
68 /* ************************************************************************* */
69 // Simulate time-of-arrival measurements for a single event
70 vector<double> simulateTOA(const vector<Point3>& microphones,
71  const Event& event) {
72  size_t K = microphones.size();
73  vector<double> simulatedTOA(K);
74  for (size_t i = 0; i < K; i++) {
75  simulatedTOA[i] = kTimeOfArrival(event, microphones[i]);
76  }
77  return simulatedTOA;
78 }
79 
80 /* ************************************************************************* */
81 // Simulate time-of-arrival measurements for an entire trajectory
82 vector<vector<double>> simulateTOA(const vector<Point3>& microphones,
83  const vector<Event>& trajectory) {
84  vector<vector<double>> simulatedTOA;
85  for (auto event : trajectory) {
86  simulatedTOA.push_back(simulateTOA(microphones, event));
87  }
88  return simulatedTOA;
89 }
90 
91 /* ************************************************************************* */
92 // create factor graph
93 NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
94  const vector<vector<double>>& simulatedTOA) {
96 
97  // Create a noise model for the TOA error
98  auto model = noiseModel::Isotropic::Sigma(1, 0.5 * ms);
99 
100  size_t K = microphones.size();
101  size_t key = 0;
102  for (auto toa : simulatedTOA) {
103  for (size_t i = 0; i < K; i++) {
104  graph.emplace_shared<TOAFactor>(key, microphones[i], toa[i], model);
105  }
106  key += 1;
107  }
108  return graph;
109 }
110 
111 /* ************************************************************************* */
112 // create initial estimate for n events
114  Values initial;
115 
116  Event zero;
117  for (size_t key = 0; key < n; key++) {
118  initial.insert(key, zero);
119  }
120  return initial;
121 }
122 
123 /* ************************************************************************* */
124 int main(int argc, char* argv[]) {
125  // Create microphones
126  auto microphones = defineMicrophones();
127  size_t K = microphones.size();
128  for (size_t i = 0; i < K; i++) {
129  cout << "mic" << i << " = " << microphones[i] << endl;
130  }
131 
132  // Create a ground truth trajectory
133  const size_t n = 5;
134  auto groundTruth = createTrajectory(n);
135 
136  // Simulate time-of-arrival measurements
137  auto simulatedTOA = simulateTOA(microphones, groundTruth);
138  for (size_t key = 0; key < n; key++) {
139  for (size_t i = 0; i < K; i++) {
140  cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms"
141  << endl;
142  }
143  }
144 
145  // Create factor graph
146  auto graph = createGraph(microphones, simulatedTOA);
147 
148  // Create initial estimate
149  auto initialEstimate = createInitialEstimate(n);
150  initialEstimate.print("Initial Estimate:\n");
151 
152  // Optimize using Levenberg-Marquardt optimization.
154  params.setAbsoluteErrorTol(1e-10);
155  params.setVerbosityLM("SUMMARY");
156  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
157  Values result = optimizer.optimize();
158  result.print("Final Result:\n");
159 }
160 /* ************************************************************************* */
NonlinearFactorGraph createGraph(const vector< Point3 > &microphones, const vector< vector< double >> &simulatedTOA)
vector< double > simulateTOA(const vector< Point3 > &microphones, const Event &event)
static const double cm
virtual const Values & optimize()
Factor Graph consisting of non-linear factors.
vector< Event > createTrajectory(size_t n)
noiseModel::Diagonal::shared_ptr model
void insert(Key j, const Value &val)
Definition: Values.cpp:140
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:271
int n
"Time of Arrival" factor
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Values initial
Definition: Half.h:150
static const double ms
NonlinearFactorGraph graph
int main(int argc, char *argv[])
Values createInitialEstimate(size_t n)
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
Values result
A "Time of Arrival" factor - so little code seems hardly worth it :-)
Definition: TOAFactor.h:28
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static SmartStereoProjectionParams params
traits
Definition: chartTesting.h:28
Time of arrival to given sensor.
Definition: Event.h:87
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:77
static const TimeOfArrival kTimeOfArrival(330)
void setVerbosityLM(const std::string &s)
vector< Point3 > defineMicrophones()
Vector3 Point3
Definition: Point3.h:35
static const double timeOfEvent
Definition: testEvent.cpp:37


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:50:34