#include <NonlinearFactor.h>

Inheritance diagram for gtsam::NoiseModelFactorN< ValueTypes >:

[legend]

Public Types
enum	{ N = sizeof...(ValueTypes) }
	N is the number of variables (N-way factor) More...

template<int I, typename = IndexIsValid<I>>
using	ValueType = typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type

Public Types inherited from gtsam::NoiseModelFactor
typedef std::shared_ptr< This >	shared_ptr

Public Types inherited from gtsam::NonlinearFactor
typedef std::shared_ptr< This >	shared_ptr

Public Types inherited from gtsam::Factor
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

	~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 `key1()` -> `key<1>()`
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

Public Member Functions inherited from gtsam::NoiseModelFactor
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 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

Public Member Functions inherited from gtsam::NonlinearFactor
	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

Public Member Functions inherited from gtsam::Factor
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 ()

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 >

Protected Types inherited from gtsam::NoiseModelFactor
typedef NonlinearFactor	Base

typedef NoiseModelFactor	This

Protected Types inherited from gtsam::NonlinearFactor
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
Protected Member Functions inherited from gtsam::NoiseModelFactor
	NoiseModelFactor (const SharedNoiseModel &noiseModel)

Protected Member Functions inherited from gtsam::Factor
	Factor ()

template<typename CONTAINER >
	Factor (const CONTAINER &keys)

template<typename ITERATOR >
	Factor (ITERATOR first, ITERATOR last)

Static Protected Member Functions inherited from gtsam::Factor
template<typename CONTAINER >
static Factor	FromKeys (const CONTAINER &keys)

template<typename ITERATOR >
static Factor	FromIterators (ITERATOR first, ITERATOR last)

Protected Attributes inherited from gtsam::NoiseModelFactor
SharedNoiseModel	noiseModel_

Protected Attributes inherited from gtsam::Factor
KeyVector	keys_
	The keys involved in this factor. More...

Detailed Description

template<class... ValueTypes>
class gtsam::NoiseModelFactorN< ValueTypes >

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:

class MyFactor : public NoiseModelFactorN<Pose3, Point3> {
 public:
  using Base = NoiseModelFactorN<Pose3, Point3>;
  MyFactor(Key pose_key, Key point_key, const SharedNoiseModel& noiseModel)
      : Base(noiseModel, pose_key, point_key) {}
  Vector evaluateError(
      const Pose3& T, const Point3& p,
      OptionalMatrixType H_T = OptionalNone,
      OptionalMatrixType H_p = OptionalNone) const override {
    Matrix36 t_H_T;  // partial derivative of translation w.r.t. pose T
    // Only compute t_H_T if needed:
    Point3 t = T.translation(H_T ? &t_H_T : 0);
    double a = t(0); // a_H_t = [1, 0, 0]
    double b = p(0); // b_H_p = [1, 0, 0]
    double error = a - b; // H_a = 1, H_b = -1
    // H_T = H_a * a_H_t * t_H_T = the first row of t_H_T
    if (H_T) *H_T = (Matrix(1, 6) << t_H_T.row(0)).finished();
    // H_p = H_b * b_H_p = -1 * [1, 0, 0]
    if (H_p) *H_p = (Matrix(1, 3) << -1., 0., 0.).finished();
    return Vector1(error);
  }
};
// Unit Test
TEST(NonlinearFactor, MyFactor) {
  MyFactor f(X(1), X(2), noiseModel::Unit::Create(1));
  EXPECT_DOUBLES_EQUAL(-8., f.evaluateError(Pose3(), Point3(8., 7., 6.))(0),
                       1e-9);
  Values values;
  values.insert(X(1), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(1, 2, 3)));
  values.insert(X(2), Point3(1, 2, 3));
  EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-5, 1e-5);
}

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 431 of file NonlinearFactor.h.

Member Typedef Documentation

◆ AreAllMatrixPtrs

template<class... ValueTypes>

template<typename Ret , typename ... Args>

using gtsam::NoiseModelFactorN< ValueTypes >::AreAllMatrixPtrs = std::enable_if_t<(... && (IsMatrixPointer<Args>::value || IsNullpointer<Args>::value)), Ret>

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 481 of file NonlinearFactor.h.

◆ AreAllMatrixRefs

template<class... ValueTypes>

template<typename Ret , typename ... Args>

using gtsam::NoiseModelFactorN< ValueTypes >::AreAllMatrixRefs = std::enable_if_t<(... && std::is_convertible<Args, Matrix&>::value), Ret>

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 467 of file NonlinearFactor.h.

◆ Base

template<class... ValueTypes>

using gtsam::NoiseModelFactorN< ValueTypes >::Base = NoiseModelFactor

protected

Definition at line 441 of file NonlinearFactor.h.

◆ ContainerElementType

template<class... ValueTypes>

template<typename Container >

using gtsam::NoiseModelFactorN< ValueTypes >::ContainerElementType = typename std::decay<decltype(*std::declval<Container>().begin())>::type

protected

Definition at line 457 of file NonlinearFactor.h.

◆ IndexIsValid

template<class... ValueTypes>

template<int I>

using gtsam::NoiseModelFactorN< ValueTypes >::IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N), void>::type

protected

Definition at line 453 of file NonlinearFactor.h.

◆ IsContainerOfKeys

template<class... ValueTypes>

template<typename Container >

using gtsam::NoiseModelFactorN< ValueTypes >::IsContainerOfKeys = IsConvertible<ContainerElementType<Container>, Key>

protected

Definition at line 459 of file NonlinearFactor.h.

◆ IsConvertible

template<class... ValueTypes>

template<typename From , typename To >

using gtsam::NoiseModelFactorN< ValueTypes >::IsConvertible = typename std::enable_if<std::is_convertible<From, To>::value, void>::type

protected

Definition at line 449 of file NonlinearFactor.h.

◆ IsMatrixPointer

template<class... ValueTypes>

template<typename Arg >

using gtsam::NoiseModelFactorN< ValueTypes >::IsMatrixPointer = std::is_same<typename std::decay_t<Arg>, Matrix*>

protected

Definition at line 470 of file NonlinearFactor.h.

◆ IsNullpointer

template<class... ValueTypes>

template<typename Arg >

using gtsam::NoiseModelFactorN< ValueTypes >::IsNullpointer = std::is_same<typename std::decay_t<Arg>, std::nullptr_t>

protected

Definition at line 473 of file NonlinearFactor.h.

◆ KeyType

template<class... ValueTypes>

template<typename T >

using gtsam::NoiseModelFactorN< ValueTypes >::KeyType = Key

protected

Definition at line 494 of file NonlinearFactor.h.

◆ MatrixTypeT

template<class... ValueTypes>

template<typename T = void>

using gtsam::NoiseModelFactorN< ValueTypes >::MatrixTypeT = Matrix

protected

Definition at line 501 of file NonlinearFactor.h.

◆ OptionalMatrixTypeT

template<class... ValueTypes>

template<typename T = void>

using gtsam::NoiseModelFactorN< ValueTypes >::OptionalMatrixTypeT = Matrix*

protected

Definition at line 489 of file NonlinearFactor.h.

◆ This

template<class... ValueTypes>

using gtsam::NoiseModelFactorN< ValueTypes >::This = NoiseModelFactorN<ValueTypes...>

protected

Definition at line 442 of file NonlinearFactor.h.

◆ ValueType

template<class... ValueTypes>

template<int I, typename = IndexIsValid<I>>

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,

using Factor = NoiseModelFactorN<Pose3, Point3>;
Factor::ValueType<1>  // Pose3
Factor::ValueType<2>  // Point3
// Factor::ValueType<0> // ERROR!  Will not compile.
// Factor::ValueType<3> // ERROR!  Will not compile.

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 525 of file NonlinearFactor.h.

Member Enumeration Documentation

◆ anonymous enum

template<class... ValueTypes>

anonymous enum

N is the number of variables (N-way factor)

Enumerator
N

Definition at line 436 of file NonlinearFactor.h.

Constructor & Destructor Documentation

◆ NoiseModelFactorN() [1/3]

template<class... ValueTypes>

gtsam::NoiseModelFactorN< ValueTypes >::NoiseModelFactorN ( )

inline

Default Constructor for I/O.

Definition at line 533 of file NonlinearFactor.h.

◆ NoiseModelFactorN() [2/3]

template<class... ValueTypes>

gtsam::NoiseModelFactorN< ValueTypes >::NoiseModelFactorN	(	const SharedNoiseModel &	noiseModel,
		KeyType< ValueTypes >...	keys
	)

inline

Constructor. Example usage: NoiseModelFactorN(noise, key1, key2, ..., keyN)

Parameters

noiseModel	Shared pointer to noise model.
keys	Keys for the variables in this factor, passed in as separate arguments.

Definition at line 542 of file NonlinearFactor.h.

◆ NoiseModelFactorN() [3/3]

template<class... ValueTypes>

template<typename CONTAINER = std::initializer_list<Key>, typename = IsContainerOfKeys<CONTAINER>>

gtsam::NoiseModelFactorN< ValueTypes >::NoiseModelFactorN	(	const SharedNoiseModel &	noiseModel,
		CONTAINER	keys
	)

inline

Constructor. Example usage: NoiseModelFactorN(noise, {key1, key2, ..., keyN}) Example usage: NoiseModelFactorN(noise, keys) where keys is a vector<Key>

Parameters

noiseModel	Shared pointer to noise model.
keys	A container of keys for the variables in this factor.

Definition at line 555 of file NonlinearFactor.h.

◆ ~NoiseModelFactorN()

template<class... ValueTypes>

gtsam::NoiseModelFactorN< ValueTypes >::~NoiseModelFactorN ( )

inlineoverride

Definition at line 565 of file NonlinearFactor.h.

Member Function Documentation

◆ evaluateError() [1/5]

template<class... ValueTypes>

virtual Vector gtsam::NoiseModelFactorN< ValueTypes >::evaluateError	(	const ValueTypes &...	x,
		OptionalMatrixTypeT< ValueTypes >...	H
	)		const

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:

Vector evaluateError(
    const Pose3& x1, const Point3& x2,
    OptionalMatrixType H1 = OptionalNone,
    OptionalMatrixType H2 = OptionalNone) const override { ... }

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.

Parameters

	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).

◆ evaluateError() [2/5]

template<class... ValueTypes>

Vector gtsam::NoiseModelFactorN< ValueTypes >::evaluateError	(	const ValueTypes &...	x,
		MatrixTypeT< ValueTypes > &...	H
	)		const

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 649 of file NonlinearFactor.h.

◆ evaluateError() [3/5]

template<class... ValueTypes>

Vector gtsam::NoiseModelFactorN< ValueTypes >::evaluateError ( const ValueTypes &... x ) const

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 663 of file NonlinearFactor.h.

◆ evaluateError() [4/5]

template<class... ValueTypes>

template<typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>

AreAllMatrixRefs<Vector, OptionalJacArgs...> gtsam::NoiseModelFactorN< ValueTypes >::evaluateError	(	const ValueTypes &...	x,
		OptionalJacArgs &&...	H
	)		const

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 672 of file NonlinearFactor.h.

◆ evaluateError() [5/5]

template<class... ValueTypes>

template<typename... OptionalJacArgs, typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>

AreAllMatrixPtrs<Vector, OptionalJacArgs...> gtsam::NoiseModelFactorN< ValueTypes >::evaluateError	(	const ValueTypes &...	x,
		OptionalJacArgs &&...	H
	)		const

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 682 of file NonlinearFactor.h.

◆ key()

template<class... ValueTypes>

template<int I = 1>

Key gtsam::NoiseModelFactorN< ValueTypes >::key ( ) const

inline

Returns a key. Usage: key<I>() returns the I'th key. I is 1-indexed for backwards compatibility/consistency! So for example,

NoiseModelFactorN<Pose3, Point3> factor(noise, key1, key2);
key<1>()  // = key1
key<2>()  // = key2
// key<0>()  // ERROR!  Will not compile
// key<3>()  // ERROR!  Will not compile

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 582 of file NonlinearFactor.h.

◆ key1()

template<class... ValueTypes>

Key gtsam::NoiseModelFactorN< ValueTypes >::key1 ( ) const

inline

Definition at line 731 of file NonlinearFactor.h.

◆ key2()

template<class... ValueTypes>

template<int I = 2>

Key gtsam::NoiseModelFactorN< ValueTypes >::key2 ( ) const

inline

Definition at line 735 of file NonlinearFactor.h.

◆ key3()

template<class... ValueTypes>

template<int I = 3>

Key gtsam::NoiseModelFactorN< ValueTypes >::key3 ( ) const

inline

Definition at line 740 of file NonlinearFactor.h.

◆ key4()

template<class... ValueTypes>

template<int I = 4>

Key gtsam::NoiseModelFactorN< ValueTypes >::key4 ( ) const

inline

Definition at line 745 of file NonlinearFactor.h.

◆ key5()

template<class... ValueTypes>

template<int I = 5>

Key gtsam::NoiseModelFactorN< ValueTypes >::key5 ( ) const

inline

Definition at line 750 of file NonlinearFactor.h.

◆ key6()

template<class... ValueTypes>

template<int I = 6>

Key gtsam::NoiseModelFactorN< ValueTypes >::key6 ( ) const

inline

Definition at line 755 of file NonlinearFactor.h.

◆ unwhitenedError() [1/2]

template<class... ValueTypes>

Vector gtsam::NoiseModelFactorN< ValueTypes >::unwhitenedError	(	const Values &	x,
		OptionalMatrixVecType	H = `nullptr`
	)		const

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:

gtsam::Values values;
values.insert(...) // populate values
std::vector<Matrix> Hs(2); // this will be an optional output argument
const Vector error = factor.unwhitenedError(values, Hs);

Parameters

[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 606 of file NonlinearFactor.h.

◆ unwhitenedError() [2/2]

template<class... ValueTypes>

template<std::size_t... Indices>

Vector gtsam::NoiseModelFactorN< ValueTypes >::unwhitenedError	(	gtsam::index_sequence< Indices... >	,
		const Values &	x,
		OptionalMatrixVecType	H = `nullptr`
	)		const

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 701 of file NonlinearFactor.h.

The documentation for this class was generated from the following file:

NonlinearFactor.h

Public Types

Public Member Functions

Protected Types

Private Member Functions

Additional Inherited Members

Detailed Description

template<class... ValueTypes> class gtsam::NoiseModelFactorN< ValueTypes >

Member Typedef Documentation

◆ AreAllMatrixPtrs

◆ AreAllMatrixRefs

◆ Base

◆ ContainerElementType

◆ IndexIsValid

◆ IsContainerOfKeys

◆ IsConvertible

◆ IsMatrixPointer

◆ IsNullpointer

◆ KeyType

◆ MatrixTypeT

◆ OptionalMatrixTypeT

◆ This

◆ ValueType

Member Enumeration Documentation

◆ anonymous enum

Constructor & Destructor Documentation

◆ NoiseModelFactorN() [1/3]

◆ NoiseModelFactorN() [2/3]

◆ NoiseModelFactorN() [3/3]

◆ ~NoiseModelFactorN()

Member Function Documentation

◆ evaluateError() [1/5]

◆ evaluateError() [2/5]

◆ evaluateError() [3/5]

◆ evaluateError() [4/5]

◆ evaluateError() [5/5]

◆ key()

◆ key1()

◆ key2()

◆ key3()

◆ key4()

◆ key5()

◆ key6()

◆ unwhitenedError() [1/2]

◆ unwhitenedError() [2/2]

template<class... ValueTypes>
class gtsam::NoiseModelFactorN< ValueTypes >