GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
A "Time of Arrival" factor - so little code seems hardly worth it :-) More...
#include <TOAFactor.h>
Public Types | |
typedef std::shared_ptr< ExpressionFactor< double > > | shared_ptr |
typedef KeyVector::iterator | iterator |
Iterator over keys. | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. | |
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) | |
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 | |
bool | equals (const NonlinearFactor &f, double tol) const override |
equals relies on Testable traits being defined for T | |
Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const override |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
gtsam::NonlinearFactor::shared_ptr | clone () const override |
size_t | dim () const override |
const SharedNoiseModel & | noiseModel () const |
access to the noise model | |
Vector | whitenedError (const Values &c) const |
Vector | unweightedWhitenedError (const Values &c) const |
double | weight (const Values &c) const |
double | error (const Values &c) const override |
shared_ptr | cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const |
Testable | |
bool | equals (const This &other, double tol=1e-9) const |
check equality | |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys | |
Standard Interface | |
double | error (const HybridValues &c) const override |
virtual bool | active (const Values &) const |
virtual shared_ptr | rekey (const std::map< Key, Key > &rekey_mapping) const |
virtual shared_ptr | rekey (const KeyVector &new_keys) const |
virtual bool | sendable () const |
Standard Interface | |
bool | empty () const |
Whether the factor is empty (involves zero variables). | |
Key | front () const |
First key. | |
Key | back () const |
Last key. | |
const_iterator | find (Key key) const |
find | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
Advanced Interface | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Static Public Member Functions | |
static void | InsertEvent (Key key, const Event &event, std::shared_ptr< Values > values) |
Protected Types | |
typedef ExpressionFactor< double > | This |
typedef NonlinearFactor | Base |
Protected Member Functions | |
void | initialize (const Expression< double > &expression) |
Initialize with constructor arguments. | |
virtual Expression< double > | expression () const |
Static Protected Member Functions | |
Standard Constructors | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
Protected Attributes | |
double | measured_ |
the measurement to be compared with the expression | |
Expression< double > | expression_ |
the expression that is AD enabled | |
FastVector< int > | dims_ |
dimensions of the Jacobian matrices | |
SharedNoiseModel | noiseModel_ |
KeyVector | keys_ |
The keys involved in this factor. | |
Static Protected Attributes | |
static const int | Dim |
A "Time of Arrival" factor - so little code seems hardly worth it :-)
|
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 |
|
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 |
|
inlinevirtualinherited |
Checks whether a factor should be used based on a set of values. This is primarily used to implement inequality constraints that require a variable active set. For all others, the default implementation returning true solves this problem.
In an inequality/bounding constraint, this active() returns true when the constraint is NOT fulfilled.
Reimplemented in gtsam::BoundingConstraint2< VALUE1, VALUE2 >, gtsam::AntiFactor, and gtsam::BoundingConstraint1< VALUE >.
|
inlineinherited |
Iterator at beginning of involved variable keys
|
inlineinherited |
Iterator at beginning of involved variable keys
|
inlineoverridevirtualinherited |
Reimplemented from gtsam::NonlinearFactor.
|
inherited |
Creates a shared_ptr clone of the factor with a new noise model
|
inlineoverridevirtualinherited |
get the dimension of the factor (number of rows on linearization)
Implements gtsam::NonlinearFactor.
|
inlineinherited |
Iterator at end of involved variable keys
|
inlineinherited |
Iterator at end of involved variable keys
|
overridevirtualinherited |
All factor types need to implement an error function. In factor graphs, this is the negative log-likelihood.
Reimplemented from gtsam::Factor.
|
overridevirtualinherited |
Calculate the error of the factor. This is the log-likelihood, e.g. \( 0.5(h(x)-z)^2/\sigma^2 \) in case of Gaussian. In this class, we take the raw prediction error \( h(x)-z \), ask the noise model to transform it to \( (h(x)-z)^2/\sigma^2 \), and then multiply by 0.5.
Reimplemented from gtsam::NonlinearFactor.
|
inlineprotectedvirtualinherited |
Recreate expression from keys_ and measured_, used in load below. Needed to deserialize a derived factor
|
inlinestaticprotectedinherited |
Construct factor from iterator keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.
|
inlinestaticprotectedinherited |
Construct factor from container of keys. This is called internally from derived factor static factor methods, as a workaround for not being able to call the protected constructors above.
|
inlineinherited |
|
inlineoverridevirtualinherited |
linearize to a GaussianFactor
Reimplemented from gtsam::NoiseModelFactor.
|
inlineinherited |
return the measurement
|
virtualinherited |
Creates a shared_ptr clone of the factor with different keys using a map from old->new keys
Reimplemented in gtsam::LinearContainerFactor.
|
virtualinherited |
Clones a factor and fully replaces its keys
new_keys | is the full replacement set of keys |
Reimplemented in gtsam::LinearContainerFactor.
|
inlinevirtualinherited |
Should the factor be evaluated in the same thread as the caller This is to enable factors that has shared states (like the Python GIL lock)
Reimplemented in gtsam::CustomFactor.
|
inlineinherited |
|
inherited |
Vector of errors, whitened, but unweighted by any loss function
|
inlineoverridevirtualinherited |
Error function without the NoiseModel, \( z-h(x) -> Local(h(x),z) \). We override this method to provide both the function evaluation and its derivative(s) in H.
Implements gtsam::NoiseModelFactor.
|
inlineinherited |
support taking in the actual vector instead of the pointer as well to get access to this version of the function from derived classes one will need to use the "using" keyword and specify that like this: public: using NoiseModelFactor::unwhitenedError;
|
inherited |
Compute the effective weight of the factor from the noise model.
|
inherited |
Vector of errors, whitened This is the raw error, i.e., i.e. \( (h(x)-z)/\sigma \) in case of a Gaussian