Implementation of a discrete-conditioned hybrid factor. More...

#include <HybridNonlinearFactor.h>

Inheritance diagram for gtsam::HybridNonlinearFactor:

Public Types
using	Base = HybridFactor

using	FactorValuePairs = DecisionTree< Key, NonlinearFactorValuePair >
	typedef for DecisionTree which has Keys as node labels and pairs of NonlinearFactor & an arbitrary scalar as leaf nodes. More...

using	shared_ptr = std::shared_ptr< HybridNonlinearFactor >

using	sharedFactor = std::shared_ptr< NonlinearFactor >

using	This = HybridNonlinearFactor

Public Types inherited from gtsam::HybridFactor
typedef Factor	Base
	Our base class. More...

enum	Category { Category::None, Category::Discrete, Category::Continuous, Category::Hybrid }
	Enum to help with categorizing hybrid factors. More...

typedef std::shared_ptr< HybridFactor >	shared_ptr
	shared_ptr to this class More...

typedef HybridFactor	This
	This class. More...

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
size_t	dim () const
	Get the dimension of the factor (number of rows on linearization). Returns the dimension of the first component factor. More...

double	error (const HybridValues &values) const override
	Compute error of factor given hybrid values. More...

double	error (const Values &continuousValues, const DiscreteValues &discreteValues) const
	Compute error of factor given both continuous and discrete values. More...

AlgebraicDecisionTree< Key >	errorTree (const Values &continuousValues) const
	Compute error of the HybridNonlinearFactor as a tree. More...

	HybridNonlinearFactor ()=default
	Default constructor, mainly for serialization. More...

	HybridNonlinearFactor (const KeyVector &continuousKeys, const DiscreteKey &discreteKey, const std::vector< NonlinearFactor::shared_ptr > &factors)
	Construct a new HybridNonlinearFactor on a single discrete key, providing the factors for each mode m as a vector of factors ϕ_m(x). The value ϕ(x,m) for the factor is simply ϕ_m(x). More...

	HybridNonlinearFactor (const KeyVector &continuousKeys, const DiscreteKey &discreteKey, const std::vector< NonlinearFactorValuePair > &pairs)
	Construct a new HybridNonlinearFactor on a single discrete key, including a scalar error value for each mode m. The factors and scalars are provided as a vector of pairs (ϕ_m(x), E_m). The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m. More...

	HybridNonlinearFactor (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const FactorValuePairs &factors)
	Construct a new HybridNonlinearFactor on a several discrete keys M, including a scalar error value for each assignment m. The factors and scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M). The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m. More...

std::shared_ptr< HybridGaussianFactor >	linearize (const Values &continuousValues) const
	Linearize all the continuous factors to get a HybridGaussianFactor. More...

GaussianFactor::shared_ptr	linearize (const Values &continuousValues, const DiscreteValues &discreteValues) const


void	print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
	print to stdout More...

bool	equals (const HybridFactor &other, double tol=1e-9) const override
	Check equality. More...

Public Member Functions inherited from gtsam::HybridFactor
	HybridFactor ()=default

	HybridFactor (const KeyVector &keys)
	Construct hybrid factor from continuous keys. More...

	HybridFactor (const DiscreteKeys &discreteKeys)
	Construct hybrid factor from discrete keys. More...

	HybridFactor (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
	Construct a new Hybrid Factor object. More...

bool	isDiscrete () const
	True if this is a factor of discrete variables only. More...

bool	isContinuous () const
	True if this is a factor of continuous variables only. More...

bool	isHybrid () const
	True is this is a Discrete-Continuous factor. More...

size_t	nrContinuous () const
	Return the number of continuous variables in this factor. More...

const DiscreteKeys &	discreteKeys () const
	Return the discrete keys for this factor. More...

const KeyVector &	continuousKeys () const
	Return only the continuous keys for this factor. More...

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

Private Member Functions
AlgebraicDecisionTree< Key >	errorTree (const VectorValues &continuousValues) const override
	HybridFactor method implementation. Should not be used. More...

Private Attributes
FactorValuePairs	factors_
	Decision tree of nonlinear factors indexed by discrete keys. More...

Additional Inherited Members
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::HybridFactor
KeyVector	continuousKeys_
	Record continuous keys for book-keeping. More...

DiscreteKeys	discreteKeys_

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

Detailed Description

Implementation of a discrete-conditioned hybrid factor.

Implements a joint discrete-continuous factor where the discrete variable serves to "select" a hybrid component corresponding to a NonlinearFactor.

This class stores all factors as HybridFactors which can then be typecast to one of (NonlinearFactor, GaussianFactor) which can then be checked to perform the correct operation.

In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e., the negative log-likelihood for a Gaussian noise model. In hybrid factor graphs we allow adding an arbitrary scalar dependent on the discrete assignment. For example, adding a 70/30 mode probability is supported by providing the scalars $-log(.7)$ and $-log(.3)$. Note that adding a common constant will not make any difference in the optimization, so $-log(70)$ and $-log(30)$ work just as well.

Definition at line 60 of file HybridNonlinearFactor.h.

Member Typedef Documentation

◆ Base

using gtsam::HybridNonlinearFactor::Base = HybridFactor

Definition at line 62 of file HybridNonlinearFactor.h.

◆ FactorValuePairs

using gtsam::HybridNonlinearFactor::FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>

typedef for DecisionTree which has Keys as node labels and pairs of NonlinearFactor & an arbitrary scalar as leaf nodes.

Definition at line 71 of file HybridNonlinearFactor.h.

◆ shared_ptr

using gtsam::HybridNonlinearFactor::shared_ptr = std::shared_ptr<HybridNonlinearFactor>

Definition at line 64 of file HybridNonlinearFactor.h.

◆ sharedFactor

using gtsam::HybridNonlinearFactor::sharedFactor = std::shared_ptr<NonlinearFactor>

Definition at line 65 of file HybridNonlinearFactor.h.

◆ This

using gtsam::HybridNonlinearFactor::This = HybridNonlinearFactor

Definition at line 63 of file HybridNonlinearFactor.h.

Constructor & Destructor Documentation

◆ HybridNonlinearFactor() [1/4]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor ( )

default

Default constructor, mainly for serialization.

◆ HybridNonlinearFactor() [2/4]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor	(	const KeyVector &	continuousKeys,
		const DiscreteKey &	discreteKey,
		const std::vector< NonlinearFactor::shared_ptr > &	factors
	)

Construct a new HybridNonlinearFactor on a single discrete key, providing the factors for each mode m as a vector of factors ϕ_m(x). The value ϕ(x,m) for the factor is simply ϕ_m(x).

Parameters

continuousKeys	Vector of keys for continuous factors.
discreteKey	The discrete key for the "mode", indexing components.
factors	Vector of gaussian factors, one for each mode.

Definition at line 43 of file HybridNonlinearFactor.cpp.

◆ HybridNonlinearFactor() [3/4]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor	(	const KeyVector &	continuousKeys,
		const DiscreteKey &	discreteKey,
		const std::vector< NonlinearFactorValuePair > &	pairs
	)

Construct a new HybridNonlinearFactor on a single discrete key, including a scalar error value for each mode m. The factors and scalars are provided as a vector of pairs (ϕ_m(x), E_m). The value ϕ(x,m) for the factor is now ϕ_m(x) + E_m.

Parameters

continuousKeys	Vector of keys for continuous factors.
discreteKey	The discrete key for the "mode", indexing components.
pairs	Vector of gaussian factor-scalar pairs, one per mode.

Definition at line 56 of file HybridNonlinearFactor.cpp.

◆ HybridNonlinearFactor() [4/4]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor	(	const KeyVector &	continuousKeys,
		const DiscreteKeys &	discreteKeys,
		const FactorValuePairs &	factors
	)

Construct a new HybridNonlinearFactor on a several discrete keys M, including a scalar error value for each assignment m. The factors and scalars are provided as a DecisionTree<Key> of pairs (ϕ_M(x), E_M). The value ϕ(x,M) for the factor is again ϕ_m(x) + E_m.

Parameters

continuousKeys	A vector of keys representing continuous variables.
discreteKeys	Discrete variables and their cardinalities.
factors	The decision tree of nonlinear factor/scalar pairs.

Definition at line 66 of file HybridNonlinearFactor.cpp.

Member Function Documentation

◆ dim()

size_t gtsam::HybridNonlinearFactor::dim ( ) const

Get the dimension of the factor (number of rows on linearization). Returns the dimension of the first component factor.

Returns: size_t

Definition at line 101 of file HybridNonlinearFactor.cpp.

◆ equals()

bool gtsam::HybridNonlinearFactor::equals	(	const HybridFactor &	other,
		double	tol = `1e-9`
	)		const

overridevirtual

Check equality.

Reimplemented from gtsam::HybridFactor.

Definition at line 125 of file HybridNonlinearFactor.cpp.

◆ error() [1/2]

double gtsam::HybridNonlinearFactor::error ( const HybridValues & values ) const

overridevirtual

Compute error of factor given hybrid values.

Parameters

values The continuous Values and the discrete assignment.

Returns: double The error of this factor.

Reimplemented from gtsam::Factor.

Definition at line 96 of file HybridNonlinearFactor.cpp.

◆ error() [2/2]

double gtsam::HybridNonlinearFactor::error	(	const Values &	continuousValues,
		const DiscreteValues &	discreteValues
	)		const

Compute error of factor given both continuous and discrete values.

Parameters

continuousValues	The continuous Values.
discreteValues	The discrete Values.

Returns: double The error of this factor.

Definition at line 85 of file HybridNonlinearFactor.cpp.

◆ errorTree() [1/2]

AlgebraicDecisionTree< Key > gtsam::HybridNonlinearFactor::errorTree ( const Values & continuousValues ) const

Compute error of the HybridNonlinearFactor as a tree.

Parameters

continuousValues The continuous values for which to compute the error.

Returns: AlgebraicDecisionTree<Key> A decision tree with the same keys as the factor, and leaf values as the error.

Definition at line 72 of file HybridNonlinearFactor.cpp.

◆ errorTree() [2/2]

AlgebraicDecisionTree<Key> gtsam::HybridNonlinearFactor::errorTree ( const VectorValues & continuousValues ) const

inlineoverrideprivatevirtual

HybridFactor method implementation. Should not be used.

Implements gtsam::HybridFactor.

Definition at line 78 of file HybridNonlinearFactor.h.

◆ linearize() [1/2]

std::shared_ptr< HybridGaussianFactor > gtsam::HybridNonlinearFactor::linearize ( const Values & continuousValues ) const

Linearize all the continuous factors to get a HybridGaussianFactor.

Definition at line 159 of file HybridNonlinearFactor.cpp.

◆ linearize() [2/2]

GaussianFactor::shared_ptr gtsam::HybridNonlinearFactor::linearize	(	const Values &	continuousValues,
		const DiscreteValues &	discreteValues
	)		const

Linearize specific nonlinear factors based on the assignment in discreteValues.

Definition at line 151 of file HybridNonlinearFactor.cpp.

◆ print()

void gtsam::HybridNonlinearFactor::print	(	const std::string &	s = `""`,
		const KeyFormatter &	keyFormatter = `DefaultKeyFormatter`
	)		const

overridevirtual

print to stdout

Testable

Reimplemented from gtsam::HybridFactor.

Definition at line 108 of file HybridNonlinearFactor.cpp.

Member Data Documentation

◆ factors_

FactorValuePairs gtsam::HybridNonlinearFactor::factors_

private

Decision tree of nonlinear factors indexed by discrete keys.

Definition at line 75 of file HybridNonlinearFactor.h.

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

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Base

◆ FactorValuePairs

◆ shared_ptr

◆ sharedFactor

◆ This

Constructor & Destructor Documentation

◆ HybridNonlinearFactor() [1/4]

◆ HybridNonlinearFactor() [2/4]

◆ HybridNonlinearFactor() [3/4]

◆ HybridNonlinearFactor() [4/4]

Member Function Documentation

◆ dim()

◆ equals()

◆ error() [1/2]

◆ error() [2/2]

◆ errorTree() [1/2]

◆ errorTree() [2/2]

◆ linearize() [1/2]

◆ linearize() [2/2]

◆ print()

Member Data Documentation

◆ factors_