#include <NonlinearFactor.h>
Public Types | |
template<int I, typename = IndexIsValid<I>> | |
using | ValueType = typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type |
![]() | |
typedef std::shared_ptr< This > | shared_ptr |
![]() | |
typedef std::shared_ptr< This > | shared_ptr |
![]() | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. More... | |
typedef KeyVector::iterator | iterator |
Iterator over keys. More... | |
Public Member Functions | |
template<int I = 1> | |
Key | key () const |
virtual Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const=0 |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
~NoiseModelFactorN () override | |
Constructors | |
NoiseModelFactorN () | |
Default Constructor for I/O. More... | |
NoiseModelFactorN (const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys) | |
template<typename CONTAINER = std::initializer_list<Key>, typename = IsContainerOfKeys<CONTAINER>> | |
NoiseModelFactorN (const SharedNoiseModel &noiseModel, CONTAINER keys) | |
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 |
template<typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>> | |
AreAllMatrixRefs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
template<typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>> | |
AreAllMatrixPtrs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
Shortcut functions <tt>key1()</tt> -> <tt>key\<1\>()</tt> | |
Key | key1 () const |
template<int I = 2> | |
Key | key2 () const |
template<int I = 3> | |
Key | key3 () const |
template<int I = 4> | |
Key | key4 () const |
template<int I = 5> | |
Key | key5 () const |
template<int I = 6> | |
Key | key6 () const |
![]() | |
shared_ptr | cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const |
size_t | dim () const override |
bool | equals (const NonlinearFactor &f, double tol=1e-9) const override |
double | error (const HybridValues &c) const override |
virtual double | error (const Values &c) const |
double | error (const Values &c) const override |
std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
const SharedNoiseModel & | noiseModel () const |
access to the noise model More... | |
NoiseModelFactor () | |
template<typename CONTAINER > | |
NoiseModelFactor (const SharedNoiseModel &noiseModel, const CONTAINER &keys) | |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
Vector | unweightedWhitenedError (const Values &c) const |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
double | weight (const Values &c) const |
Vector | whitenedError (const Values &c) const |
~NoiseModelFactor () override | |
![]() | |
NonlinearFactor () | |
template<typename CONTAINER > | |
NonlinearFactor (const CONTAINER &keys) | |
double | error (const HybridValues &c) const override |
virtual bool | active (const Values &) const |
virtual shared_ptr | clone () 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 |
![]() | |
virtual | ~Factor ()=default |
Default destructor. More... | |
bool | empty () const |
Whether the factor is empty (involves zero variables). More... | |
Key | front () const |
First key. More... | |
Key | back () const |
Last key. More... | |
const_iterator | find (Key key) const |
find More... | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. More... | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys More... | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Static Public Attributes | |
constexpr static auto | N = sizeof...(ValueTypes) |
N is the number of variables (N-way factor) More... | |
Protected Types | |
using | Base = NoiseModelFactor |
template<typename T > | |
using | KeyType = Key |
template<typename T = void> | |
using | MatrixTypeT = Matrix |
template<typename T = void> | |
using | OptionalMatrixTypeT = Matrix * |
using | This = NoiseModelFactorN< ValueTypes... > |
SFINAE aliases | |
template<typename From , typename To > | |
using | IsConvertible = typename std::enable_if< std::is_convertible< From, To >::value, void >::type |
template<int I> | |
using | IndexIsValid = typename std::enable_if<(I >=1) &&(I<=N), void >::type |
template<typename Container > | |
using | ContainerElementType = typename std::decay< decltype(*std::declval< Container >().begin())>::type |
template<typename Container > | |
using | IsContainerOfKeys = IsConvertible< ContainerElementType< Container >, Key > |
template<typename Ret , typename ... Args> | |
using | AreAllMatrixRefs = std::enable_if_t<(... &&std::is_convertible< Args, Matrix & >::value), Ret > |
template<typename Arg > | |
using | IsMatrixPointer = std::is_same< typename std::decay_t< Arg >, Matrix * > |
template<typename Arg > | |
using | IsNullpointer = std::is_same< typename std::decay_t< Arg >, std::nullptr_t > |
template<typename Ret , typename ... Args> | |
using | AreAllMatrixPtrs = std::enable_if_t<(... &&(IsMatrixPointer< Args >::value||IsNullpointer< Args >::value)), Ret > |
![]() | |
typedef NonlinearFactor | Base |
typedef NoiseModelFactor | This |
![]() | |
typedef Factor | Base |
typedef NonlinearFactor | This |
Private Member Functions | |
template<std::size_t... Indices> | |
Vector | unwhitenedError (gtsam::index_sequence< Indices... >, const Values &x, OptionalMatrixVecType H=nullptr) const |
Additional Inherited Members | |
![]() | |
NoiseModelFactor (const SharedNoiseModel &noiseModel) | |
![]() | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
![]() | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
![]() | |
SharedNoiseModel | noiseModel_ |
![]() | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
A convenient base class for creating your own NoiseModelFactor with n variables. To derive from this class, implement evaluateError().
For example, a 2-way factor that computes the difference in x-translation between a Pose3 and Point3 could be implemented like so:
These factors are templated on a values structure type. The values structures are typically more general than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds (Lie groups).
Definition at line 434 of file NonlinearFactor.h.
|
protected |
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.
Definition at line 484 of file NonlinearFactor.h.
|
protected |
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.
Definition at line 470 of file NonlinearFactor.h.
|
protected |
Definition at line 444 of file NonlinearFactor.h.
|
protected |
Definition at line 460 of file NonlinearFactor.h.
|
protected |
Definition at line 456 of file NonlinearFactor.h.
|
protected |
Definition at line 462 of file NonlinearFactor.h.
|
protected |
Definition at line 452 of file NonlinearFactor.h.
|
protected |
Definition at line 473 of file NonlinearFactor.h.
|
protected |
Definition at line 476 of file NonlinearFactor.h.
|
protected |
Definition at line 497 of file NonlinearFactor.h.
|
protected |
Definition at line 504 of file NonlinearFactor.h.
|
protected |
Definition at line 492 of file NonlinearFactor.h.
|
protected |
Definition at line 445 of file NonlinearFactor.h.
using gtsam::NoiseModelFactorN< ValueTypes >::ValueType = typename std::tuple_element<I - 1, std::tuple<ValueTypes...> >::type |
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>
.
Definition at line 528 of file NonlinearFactor.h.
|
inline |
Default Constructor for I/O.
Definition at line 536 of file NonlinearFactor.h.
|
inline |
Constructor. Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN)
noiseModel | Shared pointer to noise model. |
keys | Keys for the variables in this factor, passed in as separate arguments. |
Definition at line 545 of file NonlinearFactor.h.
|
inline |
Constructor. Example usage: NoiseModelFactorN(noise, {key1, key2, ..., keyN})
Example usage: NoiseModelFactorN(noise, keys)
where keys is a vector<Key>
noiseModel | Shared pointer to noise model. |
keys | A container of keys for the variables in this factor. |
Definition at line 558 of file NonlinearFactor.h.
|
inlineoverride |
Definition at line 568 of file NonlinearFactor.h.
|
inline |
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);
Definition at line 666 of file NonlinearFactor.h.
|
inline |
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;
Definition at line 652 of file NonlinearFactor.h.
|
inline |
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);
Definition at line 675 of file NonlinearFactor.h.
|
inline |
Some (but not all) optional Jacobians are omitted (function overload) and the jacobians are pointers to matrices. e.g. const Vector error = factor.evaluateError(pose, point, &Hpose);
Definition at line 685 of file NonlinearFactor.h.
|
pure virtual |
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). |
|
inline |
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()
.
Definition at line 585 of file NonlinearFactor.h.
|
inline |
Definition at line 734 of file NonlinearFactor.h.
|
inline |
Definition at line 738 of file NonlinearFactor.h.
|
inline |
Definition at line 743 of file NonlinearFactor.h.
|
inline |
Definition at line 748 of file NonlinearFactor.h.
|
inline |
Definition at line 753 of file NonlinearFactor.h.
|
inline |
Definition at line 758 of file NonlinearFactor.h.
|
inlineoverridevirtual |
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.
Definition at line 609 of file NonlinearFactor.h.
virtual Vector gtsam::NoiseModelFactor::unwhitenedError |
Error function without the NoiseModel, . Override this method to finish implementing an N-way factor. If the optional arguments is specified, it should compute both the function evaluation and its derivative(s) in H.
|
inline |
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;
Definition at line 264 of file NonlinearFactor.h.
|
inlineprivate |
Pack expansion with index_sequence template pattern, used to index into keys_
and H
.
Example: For NoiseModelFactorN<Pose3, Point3>
, the call would look like: const Vector error = unwhitenedError(0, 1, values, H);
Definition at line 704 of file NonlinearFactor.h.
|
inlinestaticconstexpr |
N is the number of variables (N-way factor)
Definition at line 439 of file NonlinearFactor.h.