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
12/**
13 * @file TimeOfArrivalExample.cpp
14 * @brief Track a moving object "Time of Arrival" measurements at 4
15 * microphones.
16 * @author Frank Dellaert
17 * @author Jay Chakravarty
18 * @date March 2020
19 */
20
21#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
23#include <gtsam/nonlinear/expressions.h>
24#include <gtsam/geometry/Event.h>
25#include <gtsam_unstable/slam/TOAFactor.h>
26
27#include <vector>
28
29using namespace std;
30using namespace gtsam;
31
32// units
33static const double ms = 1e-3;
34static const double cm = 1e-2;
35
36// Instantiate functor with speed of sound value
37static const TimeOfArrival kTimeOfArrival(330);
38
39/* ************************************************************************* */
40// Create microphones
41vector<Point3> defineMicrophones() {
42 const double height = 0.5;
43 vector<Point3> microphones;
44 microphones.push_back(x: Point3(0, 0, height));
45 microphones.push_back(x: Point3(403 * cm, 0, height));
46 microphones.push_back(x: Point3(403 * cm, 403 * cm, height));
47 microphones.push_back(x: Point3(0, 403 * cm, 2 * height));
48 return microphones;
49}
50
51/* ************************************************************************* */
52// Create ground truth trajectory
53vector<Event> createTrajectory(size_t n) {
54 vector<Event> trajectory;
55 double timeOfEvent = 10;
56 // simulate emitting a sound every second while moving on straight line
57 for (size_t key = 0; key < n; key++) {
58 trajectory.push_back(
59 x: Event(timeOfEvent, 245 * cm + key * 1.0, 201.5 * cm, (212 - 45) * cm));
60 timeOfEvent += 1;
61 }
62 return trajectory;
63}
64
65/* ************************************************************************* */
66// Simulate time-of-arrival measurements for a single event
67vector<double> simulateTOA(const vector<Point3>& microphones,
68 const Event& event) {
69 size_t K = microphones.size();
70 vector<double> simulatedTOA(K);
71 for (size_t i = 0; i < K; i++) {
72 simulatedTOA[i] = kTimeOfArrival(event, microphones[i]);
73 }
74 return simulatedTOA;
75}
76
77/* ************************************************************************* */
78// Simulate time-of-arrival measurements for an entire trajectory
79vector<vector<double>> simulateTOA(const vector<Point3>& microphones,
80 const vector<Event>& trajectory) {
81 vector<vector<double>> simulatedTOA;
82 for (auto event : trajectory) {
83 simulatedTOA.push_back(x: simulateTOA(microphones, event));
84 }
85 return simulatedTOA;
86}
87
88/* ************************************************************************* */
89// create factor graph
90NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
91 const vector<vector<double>>& simulatedTOA) {
92 NonlinearFactorGraph graph;
93
94 // Create a noise model for the TOA error
95 auto model = noiseModel::Isotropic::Sigma(dim: 1, sigma: 0.5 * ms);
96
97 size_t K = microphones.size();
98 size_t key = 0;
99 for (auto toa : simulatedTOA) {
100 for (size_t i = 0; i < K; i++) {
101 graph.emplace_shared<TOAFactor>(args&: key, args: microphones[i], args&: toa[i], args&: model);
102 }
103 key += 1;
104 }
105 return graph;
106}
107
108/* ************************************************************************* */
109// create initial estimate for n events
110Values createInitialEstimate(size_t n) {
111 Values initial;
112
113 Event zero;
114 for (size_t key = 0; key < n; key++) {
115 initial.insert(j: key, val: zero);
116 }
117 return initial;
118}
119
120/* ************************************************************************* */
121int main(int argc, char* argv[]) {
122 // Create microphones
123 auto microphones = defineMicrophones();
124 size_t K = microphones.size();
125 for (size_t i = 0; i < K; i++) {
126 cout << "mic" << i << " = " << microphones[i] << endl;
127 }
128
129 // Create a ground truth trajectory
130 const size_t n = 5;
131 auto groundTruth = createTrajectory(n);
132
133 // Simulate time-of-arrival measurements
134 auto simulatedTOA = simulateTOA(microphones, trajectory: groundTruth);
135 for (size_t key = 0; key < n; key++) {
136 for (size_t i = 0; i < K; i++) {
137 cout << "z_" << key << i << " = " << simulatedTOA[key][i] / ms << " ms"
138 << endl;
139 }
140 }
141
142 // Create factor graph
143 auto graph = createGraph(microphones, simulatedTOA);
144
145 // Create initial estimate
146 auto initialEstimate = createInitialEstimate(n);
147 initialEstimate.print(str: "Initial Estimate:\n");
148
149 // Optimize using Levenberg-Marquardt optimization.
150 LevenbergMarquardtParams params;
151 params.setAbsoluteErrorTol(1e-10);
152 params.setVerbosityLM("SUMMARY");
153 LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
154 Values result = optimizer.optimize();
155 result.print(str: "Final Result:\n");
156}
157/* ************************************************************************* */
158