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

#include <HybridNonlinearFactor.h>

Inheritance diagram for gtsam::HybridNonlinearFactor:

Classes
struct	ConstructorHelper

Public Types
using	Base = HybridFactor

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

using	shared_ptr = std::shared_ptr< HybridNonlinearFactor >

using	sharedFactor = std::shared_ptr< NoiseModelFactor >

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
Constructors
	HybridNonlinearFactor ()=default
	Default constructor, mainly for serialization. More...

	HybridNonlinearFactor (const DiscreteKey &discreteKey, const std::vector< NoiseModelFactor::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 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 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...

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

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

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

size_t	dim () const
	Get the dimension of the factor (number of rows on linearization). Returns the dimension of the first component factor. More...

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

Standard API
const FactorValuePairs &	factors () const
	Getter for NonlinearFactor decision tree. More...

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

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

HybridNonlinearFactor::shared_ptr	prune (const DecisionTreeFactor &discreteProbs) const
	Prune this factor based on the discrete probabilities. More...

std::shared_ptr< Factor >	restrict (const DiscreteValues &assignment) const override
	Restrict the factor to the given discrete values. 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...

	HybridNonlinearFactor (const ConstructorHelper &helper)

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

This class stores all factors as HybridFactors which can then be typecast to one of (NoiseModelFactor, 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 58 of file HybridNonlinearFactor.h.

Member Typedef Documentation

◆ Base

using gtsam::HybridNonlinearFactor::Base = HybridFactor

Definition at line 60 of file HybridNonlinearFactor.h.

◆ FactorValuePairs

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

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

Definition at line 69 of file HybridNonlinearFactor.h.

◆ shared_ptr

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

Definition at line 62 of file HybridNonlinearFactor.h.

◆ sharedFactor

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

Definition at line 63 of file HybridNonlinearFactor.h.

◆ This

using gtsam::HybridNonlinearFactor::This = HybridNonlinearFactor

Definition at line 61 of file HybridNonlinearFactor.h.

Constructor & Destructor Documentation

◆ HybridNonlinearFactor() [1/5]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor ( )

default

Default constructor, mainly for serialization.

◆ HybridNonlinearFactor() [2/5]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor	(	const DiscreteKey &	discreteKey,
		const std::vector< NoiseModelFactor::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

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

Definition at line 81 of file HybridNonlinearFactor.cpp.

◆ HybridNonlinearFactor() [3/5]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor	(	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

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

Definition at line 86 of file HybridNonlinearFactor.cpp.

◆ HybridNonlinearFactor() [4/5]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor	(	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

discreteKeys	Discrete variables and their cardinalities.
factors	The decision tree of nonlinear factor/scalar pairs.

Definition at line 91 of file HybridNonlinearFactor.cpp.

◆ HybridNonlinearFactor() [5/5]

gtsam::HybridNonlinearFactor::HybridNonlinearFactor ( const ConstructorHelper & helper )

private

Definition at line 77 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 125 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 151 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 120 of file HybridNonlinearFactor.cpp.

◆ error() [2/2]

double gtsam::HybridNonlinearFactor::error	(	const Values &	continuousValues,
		const DiscreteValues &	assignment
	)		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 109 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 96 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 76 of file HybridNonlinearFactor.h.

◆ factors()

const FactorValuePairs& gtsam::HybridNonlinearFactor::factors ( ) const

inline

Getter for NonlinearFactor decision tree.

Definition at line 176 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 184 of file HybridNonlinearFactor.cpp.

◆ linearize() [2/2]

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

Linearize specific nonlinear factors based on the assignment in discreteValues.

Definition at line 176 of file HybridNonlinearFactor.cpp.

◆ print()

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

overridevirtual

print to stdout

Reimplemented from gtsam::HybridFactor.

Definition at line 132 of file HybridNonlinearFactor.cpp.

◆ prune()

HybridNonlinearFactor::shared_ptr gtsam::HybridNonlinearFactor::prune ( const DecisionTreeFactor & discreteProbs ) const

Prune this factor based on the discrete probabilities.

Definition at line 215 of file HybridNonlinearFactor.cpp.

◆ restrict()

std::shared_ptr< Factor > gtsam::HybridNonlinearFactor::restrict ( const DiscreteValues & assignment ) const

overridevirtual

Restrict the factor to the given discrete values.

Implements gtsam::HybridFactor.

Definition at line 245 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 73 of file HybridNonlinearFactor.h.

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

Classes

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/5]

◆ HybridNonlinearFactor() [2/5]

◆ HybridNonlinearFactor() [3/5]

◆ HybridNonlinearFactor() [4/5]

◆ HybridNonlinearFactor() [5/5]

Member Function Documentation

◆ dim()

◆ equals()

◆ error() [1/2]

◆ error() [2/2]

◆ errorTree() [1/2]

◆ errorTree() [2/2]

◆ factors()

◆ linearize() [1/2]

◆ linearize() [2/2]

◆ print()

◆ prune()

◆ restrict()

Member Data Documentation

◆ factors_