gtsam 4.1.1
gtsam
TOAFactor.h
Go to the documentation of this file.
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
20#pragma once
21
22#include <gtsam/nonlinear/ExpressionFactor.h>
23#include <gtsam_unstable/geometry/Event.h>
24
25namespace gtsam {
26
28class TOAFactor : public ExpressionFactor<double> {
30
31 public:
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
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 boost::shared_ptr<Values> values) {
63 values->insert(key, event);
64 }
65};
66
67} // namespace gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:736
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
Factor that supports arbitrary expressions via AD.
Definition: ExpressionFactor.h:44
A "Time of Arrival" factor - so little code seems hardly worth it :-)
Definition: TOAFactor.h:28
TOAFactor(const Expression< Event > &eventExpression, const Expression< Point3 > &sensorExpression, double toaMeasurement, const SharedNoiseModel &model, double speed=330)
Most general constructor with two expressions.
Definition: TOAFactor.h:40
TOAFactor(const Expression< Event > &eventExpression, const Point3 &sensor, double toaMeasurement, const SharedNoiseModel &model, double speed=330)
Constructor with fixed sensor.
Definition: TOAFactor.h:55