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.

Inheritance diagram for gtsam::HybridNonlinearFactor:

Public Member Functions
Constructors
	HybridNonlinearFactor ()=default
	Default constructor, mainly for serialization.
	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).
	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.
	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.
Standard Interface
AlgebraicDecisionTree< Key >	errorTree (const Values &continuousValues) const
	Compute error of the HybridNonlinearFactor as a tree.
double	error (const Values &continuousValues, const DiscreteValues &assignment) const
	Compute error of factor given both continuous and discrete values.
double	error (const HybridValues &hybridValues) const override
	Compute error of factor given hybrid values.
size_t	dim () const
	Get the dimension of the factor (number of rows on linearization).
const FactorValuePairs &	factors () const
	Getter for NonlinearFactor decision tree.
GaussianFactor::shared_ptr	linearize (const Values &continuousValues, const DiscreteValues &assignment) const
	Linearize specific nonlinear factors based on the assignment in discreteValues.
std::shared_ptr< HybridGaussianFactor >	linearize (const Values &continuousValues) const
	Linearize all the continuous factors to get a HybridGaussianFactor.
HybridNonlinearFactor::shared_ptr	prune (const DecisionTreeFactor &discreteProbs) const
	Prune this factor based on the discrete probabilities.
std::shared_ptr< Factor >	restrict (const DiscreteValues &assignment) const override
	Restrict the factor to the given discrete values.
Testable
void	print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
	print to stdout
bool	equals (const HybridFactor &other, double tol=1e-9) const override
	Check equality.
Public Member Functions inherited from gtsam::HybridFactor
	HybridFactor ()=default
	Default constructor creates empty factor.
	HybridFactor (const KeyVector &keys)
	Construct hybrid factor from continuous keys.
	HybridFactor (const DiscreteKeys &discreteKeys)
	Construct hybrid factor from discrete keys.
	HybridFactor (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
	Construct a new Hybrid Factor object.
bool	isDiscrete () const
	True if this is a factor of discrete variables only.
bool	isContinuous () const
	True if this is a factor of continuous variables only.
bool	isHybrid () const
	True is this is a Discrete-Continuous factor.
size_t	nrContinuous () const
	Return the number of continuous variables in this factor.
const DiscreteKeys &	discreteKeys () const
	Return the discrete keys for this factor.
const KeyVector &	continuousKeys () const
	Return only the continuous keys for this factor.
Public Member Functions inherited from gtsam::Factor
virtual	~Factor ()=default
	Default destructor.
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
	Iterator at beginning of involved variable keys.
const_iterator	end () const
	Iterator at end of involved variable keys.
size_t	size () const
virtual void	printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
	print only keys
bool	equals (const This &other, double tol=1e-9) const
	check equality
KeyVector &	keys ()
iterator	begin ()
	Iterator at beginning of involved variable keys.
iterator	end ()
	Iterator at end of involved variable keys.

Public Types
using	Base = HybridFactor
using	This = HybridNonlinearFactor
using	shared_ptr = std::shared_ptr<HybridNonlinearFactor>
using	sharedFactor = std::shared_ptr<NoiseModelFactor>
using	FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>
	typedef for DecisionTree which has Keys as node labels and pairs of NoiseModelFactor & an arbitrary scalar as leaf nodes.
Public Types inherited from gtsam::HybridFactor
enum class	Category { None , Discrete , Continuous , Hybrid }
	Enum to help with categorizing hybrid factors.
typedef HybridFactor	This
	This class.
typedef std::shared_ptr< HybridFactor >	shared_ptr
	shared_ptr to this class
typedef Factor	Base
	Our base class.
Public Types inherited from gtsam::Factor
typedef KeyVector::iterator	iterator
	Iterator over keys.
typedef KeyVector::const_iterator	const_iterator
	Const iterator over keys.

Additional Inherited Members
	Factor ()
	Default constructor for I/O.
template<typename CONTAINER>
	Factor (const CONTAINER &keys)
	Construct factor from container of keys.
template<typename ITERATOR>
	Factor (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys.
template<typename CONTAINER>
static Factor	FromKeys (const CONTAINER &keys)
	Construct factor from container of keys.
template<typename ITERATOR>
static Factor	FromIterators (ITERATOR first, ITERATOR last)
	Construct factor from iterator keys.
Protected Attributes inherited from gtsam::HybridFactor
DiscreteKeys	discreteKeys_
KeyVector	continuousKeys_
	Record continuous keys for book-keeping.
Protected Attributes inherited from gtsam::Factor
KeyVector	keys_
	The keys involved in this factor.

Constructor & Destructor Documentation

◆ HybridNonlinearFactor() [1/3]

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) (i.e. scalar part is 0.0).

Parameters

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

◆ HybridNonlinearFactor() [2/3]

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 nonlinear factor-scalar pairs, one per mode.

◆ HybridNonlinearFactor() [3/3]

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.

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 found in the tree. Assumes all component factors have the same dimension.

Returns: size_t

◆ equals()

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

overridevirtual

Check equality.

Reimplemented from gtsam::HybridFactor.

◆ error() [1/2]

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

overridevirtual

Compute error of factor given hybrid values.

Parameters

values The HybridValues containing continuous (gtsam::Values) and discrete assignments.

Returns: double The error of this factor.

Reimplemented from gtsam::Factor.

◆ 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 gtsam::Values.
assignment	The assignment for the discrete keys.

Returns: double The error of this factor for the given assignment.

◆ errorTree()

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

Compute error of the HybridNonlinearFactor as a tree.

Parameters

continuousValues The continuous gtsam::Values for which to compute the error.

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

◆ linearize()

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

Linearize specific nonlinear factors based on the assignment in discreteValues.

Parameters

continuousValues	The continuous values point to linearize around.
assignment	The discrete assignment specifying which continuous factors to linearize.

Returns: GaussianFactor::shared_ptr

◆ print()

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

overridevirtual

print to stdout

Reimplemented from gtsam::HybridFactor.

◆ prune()

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

Prune this factor based on the discrete probabilities.

Entries with probability 0 (or very small) in discreteProbs will lead to pruning of corresponding branches in this factor.

Parameters

discreteProbs A DecisionTreeFactor representing P(M) or P(M|...).

Returns: HybridNonlinearFactor::shared_ptr

◆ restrict()

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

overridevirtual

Restrict the factor to the given discrete values.

If all discrete keys in this factor are assigned, the result will be a NonlinearFactor (wrapped in a Factor::shared_ptr). Otherwise, it will be a new HybridNonlinearFactor over the remaining unassigned discrete keys.

Implements gtsam::HybridFactor.

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/hybrid/HybridNonlinearFactor.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/hybrid/HybridNonlinearFactor.cpp

Detailed Description

Public Member Functions

Public Types

Additional Inherited Members

Constructor & Destructor Documentation

◆ HybridNonlinearFactor() [1/3]

◆ HybridNonlinearFactor() [2/3]

◆ HybridNonlinearFactor() [3/3]

Member Function Documentation

◆ dim()

◆ equals()

◆ error() [1/2]

◆ error() [2/2]

◆ errorTree()

◆ linearize()

◆ print()

◆ prune()

◆ restrict()