gtsam  4.1.0
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 
25 namespace gtsam {
26 
28 class 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
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::TOAFactor
A "Time of Arrival" factor - so little code seems hardly worth it :-)
Definition: TOAFactor.h:28
gtsam::Expression< double >
gtsam::TOAFactor::TOAFactor
TOAFactor(const Expression< Event > &eventExpression, const Point3 &sensor, double toaMeasurement, const SharedNoiseModel &model, double speed=330)
Constructor with fixed sensor.
Definition: TOAFactor.h:55
gtsam::Point3
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
gtsam::TOAFactor::TOAFactor
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
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition: ExpressionFactor.h:44