GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
#include <ProjectionFactorPPP.h>
Public Types | |
typedef NoiseModelFactor3< POSE, POSE, LANDMARK > | Base |
shorthand for base class type | |
typedef ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION > | This |
shorthand for this class | |
typedef std::shared_ptr< This > | shared_ptr |
shorthand for a smart pointer to a factor | |
enum | |
N is the number of variables (N-way factor) | |
using | ValueType = typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type |
typedef KeyVector::iterator | iterator |
Iterator over keys. | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. | |
Public Member Functions | |
ProjectionFactorPPP () | |
Default constructor. | |
ProjectionFactorPPP (const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, const std::shared_ptr< CALIBRATION > &K) | |
ProjectionFactorPPP (const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key transformKey, Key pointKey, const std::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality) | |
~ProjectionFactorPPP () override | |
NonlinearFactor::shared_ptr | clone () const override |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
bool | equals (const NonlinearFactor &p, double tol=1e-9) const override |
equals | |
Vector | evaluateError (const Pose3 &pose, const Pose3 &transform, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override |
Evaluate error h(x)-z and optionally derivatives. | |
const Point2 & | measured () const |
const std::shared_ptr< CALIBRATION > | calibration () const |
bool | verboseCheirality () const |
bool | throwCheirality () const |
Key | key () const |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
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 |
std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
shared_ptr | cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const |
NoiseModelFactor methods | |
Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const override |
Virtual methods | |
virtual Vector | evaluateError (const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0 |
Vector | evaluateError (const ValueTypes &... x, MatrixTypeT< ValueTypes > &... H) const |
Convenience method overloads | |
Vector | evaluateError (const ValueTypes &... x) const |
AreAllMatrixRefs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
Shortcut functions `key1()` -> `key<1>()` | |
Key | key1 () const |
Key | key2 () const |
Key | key3 () const |
Key | key4 () const |
Key | key5 () const |
Key | key6 () 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 () |
Protected Types | |
using | OptionalMatrixTypeT = Matrix * |
using | KeyType = Key |
using | MatrixTypeT = Matrix |
SFINAE aliases | |
using | IsConvertible = typename std::enable_if< std::is_convertible< From, To >::value, void >::type |
using | IndexIsValid = typename std::enable_if<(I >=1) &&(I<=N), void >::type |
using | ContainerElementType = typename std::decay< decltype(*std::declval< Container >().begin())>::type |
using | IsContainerOfKeys = IsConvertible< ContainerElementType< Container >, Key > |
using | AreAllMatrixRefs = std::enable_if_t<(... &&std::is_convertible< Args, Matrix &>::value), Ret > |
using | IsMatrixPointer = std::is_same< typename std::decay_t< Arg >, Matrix *> |
using | IsNullpointer = std::is_same< typename std::decay_t< Arg >, std::nullptr_t > |
using | AreAllMatrixPtrs = std::enable_if_t<(... &&(IsMatrixPointer< Args >::value||IsNullpointer< Args >::value)), Ret > |
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 | |
Point2 | measured_ |
2D measurement | |
std::shared_ptr< CALIBRATION > | K_ |
shared pointer to calibration object | |
bool | throwCheirality_ |
If true, rethrows Cheirality exceptions (default: false) | |
bool | verboseCheirality_ |
If true, prints text for Cheirality exceptions (default: false) | |
SharedNoiseModel | noiseModel_ |
KeyVector | keys_ |
The keys involved in this factor. | |
Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. i.e. the main building block for visual SLAM.
|
protectedinherited |
A helper alias to check if a list of args are all pointers to a matrix or not. It will be used to choose the right overload of evaluateError.
|
protectedinherited |
A helper alias to check if a list of args are all references to a matrix or not. It will be used to choose the right overload of evaluateError.
|
inherited |
The type of the I'th template param can be obtained as ValueType. I is 1-indexed for backwards compatibility/consistency! So for example,
You can also use the shortcuts X1
, ..., X6
which are the same as ValueType<1>
, ..., ValueType<6>
respectively (see detail::NoiseModelFactorAliases).
Note that, if your class is templated AND you want to use ValueType<1>
inside your class, due to dependent types you need the template
keyword: typename MyFactor<T>::template ValueType<1>
.
|
inline |
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
measured | is the 2 dimensional location of point in image (the measurement) |
model | is the standard deviation |
poseKey | is the index of the camera |
transformKey | is the index of the body-camera transform |
pointKey | is the index of the landmark |
K | shared pointer to the constant calibration |
|
inline |
Constructor with exception-handling flags TODO: Mark argument order standard (keys, measurement, parameters)
measured | is the 2 dimensional location of point in image (the measurement) |
model | is the standard deviation |
poseKey | is the index of the camera |
pointKey | is the index of the landmark |
K | shared pointer to the constant calibration |
throwCheirality | determines whether Cheirality exceptions are rethrown |
verboseCheirality | determines whether exceptions are printed for Cheirality |
|
inlineoverride |
Virtual destructor
|
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
|
inline |
return the calibration object
|
inlineoverridevirtual |
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.
|
pure virtualinherited |
Override evaluateError
to finish implementing an n-way factor.
Both the x
and H
arguments are written here as parameter packs, but when overriding this method, you probably want to explicitly write them out. For example, for a 2-way factor with variable types Pose3 and Point3, you should implement:
If any of the optional Matrix reference arguments are specified, it should compute both the function evaluation and its derivative(s) in the requested variables.
x | The values of the variables to evaluate the error for. Passed in as separate arguments. | |
[out] | H | The Jacobian with respect to each variable (optional). |
|
inlineinherited |
If all the optional arguments are matrices then redirect the call to the one which takes pointers. 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 NoiseModelFactorN<list the value types here>::evaluateError;
|
inlineinherited |
No-Jacobians requested function overload. This specializes the version below to avoid recursive calls since this is commonly used.
e.g. const Vector error = factor.evaluateError(pose, point);
|
inlineinherited |
Some (but not all) optional Jacobians are omitted (function overload) and the jacobians are l-value references to matrices. e.g. const Vector error = factor.evaluateError(pose, point, Hpose);
|
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 |
Returns a key. Usage: key<I>()
returns the I'th key. I is 1-indexed for backwards compatibility/consistency! So for example,
Note that, if your class is templated AND you are trying to call key<1>
inside your class, due to dependent types you need the template
keyword: this->key1()
.
|
inlineinherited |
|
overridevirtualinherited |
Linearize a non-linearFactorN to get a GaussianFactor, \( Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \) Hence \( b = z - h(x) = - \mathtt{error\_vector}(x) \)
Implements gtsam::NonlinearFactor.
Reimplemented in gtsam::TriangulationFactor< CAMERA >, gtsam::GeneralSFMFactor< CAMERA, LANDMARK >, gtsam::ExpressionFactor< BearingRange< A1, A2 > >, and gtsam::ExpressionFactor< double >.
|
inline |
return the measurement
|
inlineoverridevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented from gtsam::NoiseModelFactor.
|
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 |
|
inline |
return flag for throwing cheirality exceptions
|
inherited |
Vector of errors, whitened, but unweighted by any loss function
|
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;
|
inlineoverridevirtualinherited |
This implements the unwhitenedError
virtual function by calling the n-key specific version of evaluateError, which is pure virtual so must be implemented in the derived class.
Example usage:
[in] | x | A Values object containing the values of all the variables used in this factor |
[out] | H | A vector of (dynamic) matrices whose size should be equal to n. The Jacobians w.r.t. each variable will be output in this parameter. |
Implements gtsam::NoiseModelFactor.
|
inline |
return verbosity
|
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