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 <vector>
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 // units
33 static const double ms = 1e-3;
34 static const double cm = 1e-2;
35 
36 // Instantiate functor with speed of sound value
37 static const TimeOfArrival kTimeOfArrival(330);
38 
39 /* ************************************************************************* */
40 // Create microphones
41 vector<Point3> defineMicrophones() {
42  const double height = 0.5;
43  vector<Point3> microphones;
44  microphones.push_back(Point3(0, 0, height));
45  microphones.push_back(Point3(403 * cm, 0, height));
46  microphones.push_back(Point3(403 * cm, 403 * cm, height));
47  microphones.push_back(Point3(0, 403 * cm, 2 * height));
48  return microphones;
49 }
50 
51 /* ************************************************************************* */
52 // Create ground truth trajectory
53 vector<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  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
67 vector<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
79 vector<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(simulateTOA(microphones, event));
84  }
85  return simulatedTOA;
86 }
87 
88 /* ************************************************************************* */
89 // create factor graph
90 NonlinearFactorGraph createGraph(const vector<Point3>& microphones,
91  const vector<vector<double>>& simulatedTOA) {
93 
94  // Create a noise model for the TOA error
95  auto model = noiseModel::Isotropic::Sigma(1, 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>(key, microphones[i], toa[i], model);
102  }
103  key += 1;
104  }
105  return graph;
106 }
107 
108 /* ************************************************************************* */
109 // create initial estimate for n events
111  Values initial;
112 
113  Event zero;
114  for (size_t key = 0; key < n; key++) {
115  initial.insert(key, zero);
116  }
117  return initial;
118 }
119 
120 /* ************************************************************************* */
121 int 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, 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("Initial Estimate:\n");
148 
149  // Optimize using Levenberg-Marquardt optimization.
151  params.setAbsoluteErrorTol(1e-10);
152  params.setVerbosityLM("SUMMARY");
153  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
154  Values result = optimizer.optimize();
155  result.print("Final Result:\n");
156 }
157 /* ************************************************************************* */
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
main
int main(int argc, char *argv[])
Definition: TimeOfArrivalExample.cpp:121
kTimeOfArrival
static const TimeOfArrival kTimeOfArrival(330)
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::TimeOfArrival
Time of arrival to given sensor.
Definition: Event.h:87
result
Values result
Definition: OdometryOptimize.cpp:8
timeOfEvent
static const double timeOfEvent
Definition: testEvent.cpp:36
n
int n
Definition: BiCGSTAB_simple.cpp:1
expressions.h
Common expressions, both linear and non-linear.
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
zero
EIGEN_DONT_INLINE Scalar zero()
Definition: svd_common.h:296
gtsam::TOAFactor
A "Time of Arrival" factor - so little code seems hardly worth it :-)
Definition: TOAFactor.h:28
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
Event.h
Space-time event.
createTrajectory
vector< Event > createTrajectory(size_t n)
Definition: TimeOfArrivalExample.cpp:53
defineMicrophones
vector< Point3 > defineMicrophones()
Definition: TimeOfArrivalExample.cpp:41
gtsam::Event
Definition: Event.h:37
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
key
const gtsam::Symbol key('X', 0)
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
TOAFactor.h
"Time of Arrival" factor
K
#define K
Definition: igam.h:8
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
ms
static const double ms
Definition: TimeOfArrivalExample.cpp:33
std
Definition: BFloat16.h:88
gtsam.examples.PlanarManipulatorExample.trajectory
def trajectory(g0, g1, N=20)
Definition: PlanarManipulatorExample.py:50
createGraph
NonlinearFactorGraph createGraph(const vector< Point3 > &microphones, const vector< vector< double >> &simulatedTOA)
Definition: TimeOfArrivalExample.cpp:90
cm
static const double cm
Definition: TimeOfArrivalExample.cpp:34
simulateTOA
vector< double > simulateTOA(const vector< Point3 > &microphones, const Event &event)
Definition: TimeOfArrivalExample.cpp:67
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
initial
Definition: testScenarioRunner.cpp:148
createInitialEstimate
Values createInitialEstimate(size_t n)
Definition: TimeOfArrivalExample.cpp:110
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:59