1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, 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 TOAFactor.h
14 * @brief "Time of Arrival" factor
15 * @author Frank Dellaert
16 * @author Jay Chakravarty
17 * @date December 2014
18 */
19
20#pragma once
21
22#include <gtsam/nonlinear/ExpressionFactor.h>
23#include <gtsam/geometry/Event.h>
24
25namespace gtsam {
26
27/// A "Time of Arrival" factor - so little code seems hardly worth it :-)
28class TOAFactor : public ExpressionFactor<double> {
29 typedef Expression<double> Double_;
30
31 public:
32 /**
33 * Most general constructor with two expressions
34 * @param eventExpression expression yielding an event
35 * @param sensorExpression expression yielding a sensor location
36 * @param toaMeasurement time of arrival at sensor
37 * @param model noise model
38 * @param speed optional speed of signal, in m/sec
39 */
40 TOAFactor(const Expression<Event>& eventExpression,
41 const Expression<Point3>& sensorExpression, double toaMeasurement,
42 const SharedNoiseModel& model, double speed = 330)
43 : ExpressionFactor<double>(
44 model, toaMeasurement,
45 Double_(TimeOfArrival(speed), eventExpression, sensorExpression)) {}
46
47 /**
48 * Constructor with fixed sensor
49 * @param eventExpression expression yielding an event
50 * @param sensor a known sensor location
51 * @param toaMeasurement time of arrival at sensor
52 * @param model noise model
53 * @param toa optional time of arrival functor
54 */
55 TOAFactor(const Expression<Event>& eventExpression, const Point3& sensor,
56 double toaMeasurement, const SharedNoiseModel& model,
57 double speed = 330)
58 : TOAFactor(eventExpression, Expression<Point3>(sensor), toaMeasurement,
59 model, speed) {}
60
61 static void InsertEvent(Key key, const Event& event,
62 std::shared_ptr<Values> values) {
63 values->insert(j: key, val: event);
64 }
65};
66
67} // namespace gtsam
68