A "Time of Arrival" factor - so little code seems hardly worth it :-) More...
#include <TOAFactor.h>
Public Member Functions | |
TOAFactor (const Expression< Event > &eventExpression, const Expression< Point3 > &sensorExpression, double toaMeasurement, const SharedNoiseModel &model, double speed=330) | |
TOAFactor (const Expression< Event > &eventExpression, const Point3 &sensor, double toaMeasurement, const SharedNoiseModel &model, double speed=330) | |
Public Member Functions inherited from gtsam::ExpressionFactor< double > | |
gtsam::NonlinearFactor::shared_ptr | clone () const override |
bool | equals (const NonlinearFactor &f, double tol) const override |
equals relies on Testable traits being defined for T More... | |
ExpressionFactor (const SharedNoiseModel &noiseModel, const double &measurement, const Expression< double > &expression) | |
std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
const double & | measured () const |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
print relies on Testable traits being defined for T More... | |
virtual Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const =0 |
Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const override |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
~ExpressionFactor () override | |
Destructor. More... | |
Static Public Member Functions | |
static void | InsertEvent (Key key, const Event &event, std::shared_ptr< Values > values) |
Private Types | |
typedef Expression< double > | Double_ |
Additional Inherited Members | |
Public Types inherited from gtsam::ExpressionFactor< double > | |
typedef std::shared_ptr< ExpressionFactor< double > > | shared_ptr |
Protected Types inherited from gtsam::ExpressionFactor< double > | |
typedef ExpressionFactor< double > | This |
Protected Member Functions inherited from gtsam::ExpressionFactor< double > | |
virtual Expression< double > | expression () const |
ExpressionFactor () | |
ExpressionFactor (const SharedNoiseModel &noiseModel, const double &measurement) | |
Default constructor, for serialization. More... | |
void | initialize (const Expression< double > &expression) |
Initialize with constructor arguments. More... | |
Protected Attributes inherited from gtsam::ExpressionFactor< double > | |
FastVector< int > | dims_ |
dimensions of the Jacobian matrices More... | |
Expression< double > | expression_ |
the expression that is AD enabled More... | |
double | measured_ |
the measurement to be compared with the expression More... | |
Static Protected Attributes inherited from gtsam::ExpressionFactor< double > | |
static const int | Dim |
A "Time of Arrival" factor - so little code seems hardly worth it :-)
Definition at line 28 of file TOAFactor.h.
|
private |
Definition at line 29 of file TOAFactor.h.
|
inline |
Most general constructor with two expressions
eventExpression | expression yielding an event |
sensorExpression | expression yielding a sensor location |
toaMeasurement | time of arrival at sensor |
model | noise model |
speed | optional speed of signal, in m/sec |
Definition at line 40 of file TOAFactor.h.
|
inline |
Constructor with fixed sensor
eventExpression | expression yielding an event |
sensor | a known sensor location |
toaMeasurement | time of arrival at sensor |
model | noise model |
toa | optional time of arrival functor |
Definition at line 55 of file TOAFactor.h.
|
inlinestatic |
Definition at line 61 of file TOAFactor.h.