GTSAM  4.0.2
C++ library for smoothing and mapping (SAM)
Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | Friends | List of all members
gtsam::GaussianFactorGraph Class Reference

#include <GaussianFactorGraph.h>

Inheritance diagram for gtsam::GaussianFactorGraph:
Inheritance graph
[legend]
Collaboration diagram for gtsam::GaussianFactorGraph:
Collaboration graph
[legend]

Public Types

typedef GaussianFactorGraph This
 Typedef to this class.
 
typedef FactorGraph< GaussianFactorBase
 Typedef to base factor graph type.
 
typedef EliminateableFactorGraph< ThisBaseEliminateable
 Typedef to base elimination class.
 
typedef std::shared_ptr< Thisshared_ptr
 shared_ptr to this class
 
typedef KeySet Keys
 
typedef GaussianFactor FactorType
 factor type
 
typedef std::shared_ptr< GaussianFactorsharedFactor
 Shared pointer to a factor.
 
typedef sharedFactor value_type
 
typedef FastVector< sharedFactor >::iterator iterator
 
typedef FastVector< sharedFactor >::const_iterator const_iterator
 
typedef EliminationTraits< FactorGraphTypeEliminationTraitsType
 Typedef to the specific EliminationTraits for this graph.
 
typedef EliminationTraitsType::ConditionalType ConditionalType
 Conditional type stored in the Bayes net produced by elimination.
 
typedef EliminationTraitsType::BayesNetType BayesNetType
 Bayes net type produced by sequential elimination.
 
typedef EliminationTraitsType::EliminationTreeType EliminationTreeType
 Elimination tree type that can do sequential elimination of this graph.
 
typedef EliminationTraitsType::BayesTreeType BayesTreeType
 Bayes tree type produced by multifrontal elimination.
 
typedef EliminationTraitsType::JunctionTreeType JunctionTreeType
 Junction tree type that can do multifrontal elimination of this graph.
 
typedef std::pair< std::shared_ptr< ConditionalType >, std::shared_ptr< _FactorType > > EliminationResult
 
typedef std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
 The function type that does a single dense elimination step on a subgraph.
 
typedef std::optional< std::reference_wrapper< const VariableIndex > > OptionalVariableIndex
 
typedef std::optional< Ordering::OrderingTypeOptionalOrderingType
 Typedef for an optional ordering type.
 

Public Member Functions

void add (const GaussianFactor &factor)
 
void add (const sharedFactor &factor)
 
void add (const Vector &b)
 
void add (Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
void add (Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
void add (Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
template<class TERMS >
void add (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
 
Keys keys () const
 
std::map< Key, size_t > getKeyDimMap () const
 
double error (const VectorValues &x) const
 
double probPrime (const VectorValues &c) const
 
virtual GaussianFactorGraph clone () const
 
virtual GaussianFactorGraph::shared_ptr cloneToPtr () const
 
GaussianFactorGraph negate () const
 
std::shared_ptr< BayesNetTypeeliminateSequential (OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypeeliminateSequential (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypeeliminateMultifrontal (OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypeeliminateMultifrontal (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal (const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::pair< std::shared_ptr< BayesTreeType >, std::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const Ordering &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesNetTypemarginalMultifrontalBayesNet (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const Ordering &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const Ordering &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< BayesTreeTypemarginalMultifrontalBayesTree (const KeyVector &variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
std::shared_ptr< FactorGraphTypemarginal (const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
 
Constructors
 GaussianFactorGraph ()
 
 GaussianFactorGraph (std::initializer_list< sharedFactor > factors)
 
template<typename ITERATOR >
 GaussianFactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
 
template<class CONTAINER >
 GaussianFactorGraph (const CONTAINER &factors)
 
template<class DERIVEDFACTOR >
 GaussianFactorGraph (const FactorGraph< DERIVEDFACTOR > &graph)
 
Testable
bool equals (const This &fg, double tol=1e-9) const
 
Linear Algebra
std::vector< std::tuple< int, int, double > > sparseJacobian (const Ordering &ordering, size_t &nrows, size_t &ncols) const
 
std::vector< std::tuple< int, int, double > > sparseJacobian () const
 
Matrix sparseJacobian_ () const
 
Matrix augmentedJacobian (const Ordering &ordering) const
 
Matrix augmentedJacobian () const
 
std::pair< Matrix, Vector > jacobian (const Ordering &ordering) const
 
std::pair< Matrix, Vector > jacobian () const
 
Matrix augmentedHessian (const Ordering &ordering) const
 
Matrix augmentedHessian () const
 
std::pair< Matrix, Vector > hessian (const Ordering &ordering) const
 
std::pair< Matrix, Vector > hessian () const
 
virtual VectorValues hessianDiagonal () const
 
virtual std::map< Key, Matrix > hessianBlockDiagonal () const
 
VectorValues optimize (const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
 
VectorValues optimize (const Ordering &, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
 
VectorValues optimizeDensely () const
 
VectorValues gradient (const VectorValues &x0) const
 
virtual VectorValues gradientAtZero () const
 
VectorValues optimizeGradientSearch () const
 
VectorValues transposeMultiply (const Errors &e) const
 
void transposeMultiplyAdd (double alpha, const Errors &e, VectorValues &x) const
 
Errors gaussianErrors (const VectorValues &x) const
 
Errors operator* (const VectorValues &x) const
 
void multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const
 
void multiplyInPlace (const VectorValues &x, Errors &e) const
 
void multiplyInPlace (const VectorValues &x, const Errors::iterator &e) const
 
void printErrors (const VectorValues &x, const std::string &str="GaussianFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter, const std::function< bool(const Factor *, double, size_t)> &printCondition=[](const Factor *, double, size_t) { return true;}) const
 
Adding Single Factors
void reserve (size_t size)
 
IsDerived< DERIVEDFACTOR > push_back (std::shared_ptr< DERIVEDFACTOR > factor)
 Add a factor directly using a shared_ptr.
 
IsDerived< DERIVEDFACTOR > push_back (const DERIVEDFACTOR &factor)
 
IsDerived< DERIVEDFACTOR > emplace_shared (Args &&... args)
 Emplace a shared pointer to factor of given type.
 
IsDerived< DERIVEDFACTOR > add (std::shared_ptr< DERIVEDFACTOR > factor)
 add is a synonym for push_back.
 
Adding via iterators
HasDerivedElementType< ITERATOR > push_back (ITERATOR firstFactor, ITERATOR lastFactor)
 
HasDerivedValueType< ITERATOR > push_back (ITERATOR firstFactor, ITERATOR lastFactor)
 Push back many factors with an iterator (factors are copied)
 
Adding via container
HasDerivedElementType< CONTAINER > push_back (const CONTAINER &container)
 
HasDerivedValueType< CONTAINER > push_back (const CONTAINER &container)
 Push back non-pointer objects in a container (factors are copied).
 
void add (const FACTOR_OR_CONTAINER &factorOrContainer)
 
Specialized versions
std::enable_if< std::is_base_of< This, typename CLIQUE::FactorGraphType >::value >::type push_back (const BayesTree< CLIQUE > &bayesTree)
 
FactorIndices add_factors (const CONTAINER &factors, bool useEmptySlots=false)
 
Testable
virtual void print (const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
 Print out graph to std::cout, with optional key formatter.
 
bool equals (const This &fg, double tol=1e-9) const
 Check equality up to tolerance.
 
Standard Interface
size_t size () const
 
bool empty () const
 
const sharedFactor at (size_t i) const
 
sharedFactorat (size_t i)
 
const sharedFactor operator[] (size_t i) const
 
sharedFactoroperator[] (size_t i)
 
const_iterator begin () const
 
const_iterator end () const
 
sharedFactor front () const
 
sharedFactor back () const
 
double error (const HybridValues &values) const
 
Modifying Factor Graphs (imperative, discouraged)
iterator begin ()
 
iterator end ()
 
virtual void resize (size_t size)
 
void remove (size_t i)
 
void replace (size_t index, sharedFactor factor)
 
iterator erase (iterator item)
 
iterator erase (iterator first, iterator last)
 
Graph Display
void dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format, stream version.
 
std::string dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format string.
 
void saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 output to file with graphviz format.
 
Advanced Interface
size_t nrFactors () const
 
KeyVector keyVector () const
 
bool exists (size_t idx) const
 

Protected Member Functions

bool isEqual (const FactorGraph &other) const
 Check exact equality of the factor pointers. Useful for derived ==.
 

Protected Attributes

FastVector< sharedFactorfactors_
 

Friends

bool operator== (const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
 Check exact equality.
 

Detailed Description

A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. Factor == GaussianFactor VectorValues = A values structure of vectors Most of the time, linear factor graphs arise by linearizing a non-linear factor graph.

Member Typedef Documentation

◆ EliminationResult

typedef std::pair<std::shared_ptr<ConditionalType>, std::shared_ptr<_FactorType> > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::EliminationResult
inherited

The pair of conditional and remaining factor produced by a single dense elimination step on a subgraph.

◆ Keys

Return the set of variables involved in the factors (computes a set union).

◆ OptionalVariableIndex

typedef std::optional<std::reference_wrapper<const VariableIndex> > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::OptionalVariableIndex
inherited

Typedef for an optional variable index as an argument to elimination functions It is an optional to a constant reference

Constructor & Destructor Documentation

◆ GaussianFactorGraph() [1/5]

gtsam::GaussianFactorGraph::GaussianFactorGraph ( )
inline

Default constructor

◆ GaussianFactorGraph() [2/5]

gtsam::GaussianFactorGraph::GaussianFactorGraph ( std::initializer_list< sharedFactor factors)
inline

Construct from an initializer lists of GaussianFactor shared pointers. Example: GaussianFactorGraph graph = { factor1, factor2, factor3 };

◆ GaussianFactorGraph() [3/5]

template<typename ITERATOR >
gtsam::GaussianFactorGraph::GaussianFactorGraph ( ITERATOR  firstFactor,
ITERATOR  lastFactor 
)
inline

Construct from iterator over factors

◆ GaussianFactorGraph() [4/5]

template<class CONTAINER >
gtsam::GaussianFactorGraph::GaussianFactorGraph ( const CONTAINER &  factors)
inlineexplicit

Construct from container of factors (shared_ptr or plain objects)

◆ GaussianFactorGraph() [5/5]

template<class DERIVEDFACTOR >
gtsam::GaussianFactorGraph::GaussianFactorGraph ( const FactorGraph< DERIVEDFACTOR > &  graph)
inline

Implicit copy/downcast constructor to override explicit template container constructor

Member Function Documentation

◆ add() [1/8]

void gtsam::GaussianFactorGraph::add ( const GaussianFactor factor)
inline

Add a factor by value - makes a copy

◆ add() [2/8]

void gtsam::GaussianFactorGraph::add ( const sharedFactor factor)
inline

Add a factor by pointer - stores pointer without copying the factor

◆ add() [3/8]

void gtsam::GaussianFactorGraph::add ( const Vector &  b)
inline

Add a null factor

◆ add() [4/8]

void gtsam::GaussianFactorGraph::add ( Key  key1,
const Matrix &  A1,
const Vector &  b,
const SharedDiagonal &  model = SharedDiagonal() 
)
inline

Add a unary factor

◆ add() [5/8]

void gtsam::GaussianFactorGraph::add ( Key  key1,
const Matrix &  A1,
Key  key2,
const Matrix &  A2,
const Vector &  b,
const SharedDiagonal &  model = SharedDiagonal() 
)
inline

Add a binary factor

◆ add() [6/8]

void gtsam::GaussianFactorGraph::add ( Key  key1,
const Matrix &  A1,
Key  key2,
const Matrix &  A2,
Key  key3,
const Matrix &  A3,
const Vector &  b,
const SharedDiagonal &  model = SharedDiagonal() 
)
inline

Add a ternary factor

◆ add() [7/8]

template<class TERMS >
void gtsam::GaussianFactorGraph::add ( const TERMS &  terms,
const Vector &  b,
const SharedDiagonal &  model = SharedDiagonal() 
)
inline

Add an n-ary factor

◆ add() [8/8]

void gtsam::FactorGraph< GaussianFactor >::add ( const FACTOR_OR_CONTAINER &  factorOrContainer)
inlineinherited

Add a factor or container of factors, including STL collections, BayesTrees, etc.

◆ add_factors()

FactorIndices gtsam::FactorGraph< GaussianFactor >::add_factors ( const CONTAINER &  factors,
bool  useEmptySlots = false 
)
inherited

Add new factors to a factor graph and returns a list of new factor indices, optionally finding and reusing empty factor slots.

◆ at() [1/2]

const sharedFactor gtsam::FactorGraph< GaussianFactor >::at ( size_t  i) const
inlineinherited

Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).

◆ at() [2/2]

sharedFactor& gtsam::FactorGraph< GaussianFactor >::at ( size_t  i)
inlineinherited

Get a specific factor by index (this checks array bounds and may throw an exception, as opposed to operator[] which does not).

◆ augmentedHessian() [1/2]

Matrix gtsam::GaussianFactorGraph::augmentedHessian ( const Ordering ordering) const

Return a dense \( \Lambda \in \mathbb{R}^{n+1 \times n+1} \) Hessian matrix, augmented with the information vector \( \eta \). The augmented Hessian is

\[ \left[ \begin{array}{ccc} \Lambda & \eta \\ \eta^T & c \end{array} \right] \]

and the negative log-likelihood is \( \frac{1}{2} x^T \Lambda x + \eta^T x + c \).

◆ augmentedHessian() [2/2]

Matrix gtsam::GaussianFactorGraph::augmentedHessian ( ) const

Return a dense \( \Lambda \in \mathbb{R}^{n+1 \times n+1} \) Hessian matrix, augmented with the information vector \( \eta \). The augmented Hessian is

\[ \left[ \begin{array}{ccc} \Lambda & \eta \\ \eta^T & c \end{array} \right] \]

and the negative log-likelihood is \( \frac{1}{2} x^T \Lambda x + \eta^T x + c \).

◆ augmentedJacobian() [1/2]

Matrix gtsam::GaussianFactorGraph::augmentedJacobian ( const Ordering ordering) const

Return a dense \( [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \) Jacobian matrix, augmented with b with the noise models baked into A and b. The negative log-likelihood is \( \frac{1}{2} \Vert Ax-b \Vert^2 \). See also GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.

◆ augmentedJacobian() [2/2]

Matrix gtsam::GaussianFactorGraph::augmentedJacobian ( ) const

Return a dense \( [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \) Jacobian matrix, augmented with b with the noise models baked into A and b. The negative log-likelihood is \( \frac{1}{2} \Vert Ax-b \Vert^2 \). See also GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.

◆ back()

sharedFactor gtsam::FactorGraph< GaussianFactor >::back ( ) const
inlineinherited

Get the last factor

◆ begin() [1/2]

const_iterator gtsam::FactorGraph< GaussianFactor >::begin ( ) const
inlineinherited

Iterator to beginning of factors.

◆ begin() [2/2]

iterator gtsam::FactorGraph< GaussianFactor >::begin ( )
inlineinherited

non-const STL-style begin()

◆ clone()

virtual GaussianFactorGraph gtsam::GaussianFactorGraph::clone ( ) const
virtual

Clone() performs a deep-copy of the graph, including all of the factors. Cloning preserves null factors so indices for the original graph are still valid for the cloned graph.

◆ cloneToPtr()

virtual GaussianFactorGraph::shared_ptr gtsam::GaussianFactorGraph::cloneToPtr ( ) const
virtual

CloneToPtr() performs a simple assignment to a new graph and returns it. There is no preservation of null factors!

◆ eliminateMultifrontal() [1/2]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminateMultifrontal ( OptionalOrderingType  orderingType = {},
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not provided, the ordering will be computed using either COLAMD or METIS, depending on the parameter orderingType (Ordering::COLAMD or Ordering::METIS)

Example - Full Cholesky elimination in COLAMD order:

std::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateCholesky);

Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering:

VariableIndex varIndex(graph); // Build variable index
Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
std::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, {}, varIndex);

◆ eliminateMultifrontal() [2/2]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminateMultifrontal ( const Ordering ordering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not provided, the ordering will be computed using either COLAMD or METIS, depending on the parameter orderingType (Ordering::COLAMD or Ordering::METIS)

Example - Full QR elimination in specified order:

std::shared_ptr<GaussianBayesTree> result = graph.eliminateMultifrontal(EliminateQR, myOrdering);

◆ eliminatePartialMultifrontal() [1/2]

std::pair< std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType >, std::shared_ptr< GaussianFactorGraph > > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminatePartialMultifrontal ( const Ordering ordering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do multifrontal elimination of some variables, in ordering provided, to produce a Bayes tree and a remaining factor graph. This computes the factorization \( p(X) = p(A|B) p(B) \), where \( A = \) variables, \( X \) is all the variables in the factor graph, and \( B = X\backslash A \).

◆ eliminatePartialMultifrontal() [2/2]

std::pair< std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType >, std::shared_ptr< GaussianFactorGraph > > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminatePartialMultifrontal ( const KeyVector variables,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do multifrontal elimination of the given variables in an ordering computed by COLAMD to produce a Bayes tree and a remaining factor graph. This computes the factorization \( p(X) = p(A|B) p(B) \), where \( A = \) variables, \( X \) is all the variables in the factor graph, and \( B = X\backslash A \).

◆ eliminatePartialSequential() [1/2]

std::pair< std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType >, std::shared_ptr< GaussianFactorGraph > > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminatePartialSequential ( const Ordering ordering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remaining factor graph. This computes the factorization \( p(X) = p(A|B) p(B) \), where \( A = \) variables, \( X \) is all the variables in the factor graph, and \( B = X\backslash A \).

◆ eliminatePartialSequential() [2/2]

std::pair< std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType >, std::shared_ptr< GaussianFactorGraph > > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminatePartialSequential ( const KeyVector variables,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do sequential elimination of the given variables in an ordering computed by COLAMD to produce a Bayes net and a remaining factor graph. This computes the factorization \( p(X) = p(A|B) p(B) \), where \( A = \) variables, \( X \) is all the variables in the factor graph, and \( B = X\backslash A \).

◆ eliminateSequential() [1/2]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminateSequential ( OptionalOrderingType  orderingType = {},
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do sequential elimination of all variables to produce a Bayes net. If an ordering is not provided, the ordering provided by COLAMD will be used.

Example - Full Cholesky elimination in COLAMD order:

std::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateCholesky);

Example - METIS ordering for elimination

std::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(OrderingType::METIS);

Example - Reusing an existing VariableIndex to improve performance, and using COLAMD ordering:

VariableIndex varIndex(graph); // Build variable index
Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
std::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(EliminateQR, varIndex, std::nullopt);

◆ eliminateSequential() [2/2]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::eliminateSequential ( const Ordering ordering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Do sequential elimination of all variables to produce a Bayes net.

Example - Full QR elimination in specified order:

std::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR);

Example - Reusing an existing VariableIndex to improve performance:

VariableIndex varIndex(graph); // Build variable index
Data data = otherFunctionUsingVariableIndex(graph, varIndex); // Other code that uses variable index
std::shared_ptr<GaussianBayesNet> result = graph.eliminateSequential(myOrdering, EliminateQR, varIndex, std::nullopt);

◆ empty()

bool gtsam::FactorGraph< GaussianFactor >::empty ( ) const
inlineinherited

Check if the graph is empty (null factors set by remove() will cause this to return false).

◆ end() [1/2]

const_iterator gtsam::FactorGraph< GaussianFactor >::end ( ) const
inlineinherited

Iterator to end of factors.

◆ end() [2/2]

iterator gtsam::FactorGraph< GaussianFactor >::end ( )
inlineinherited

non-const STL-style end()

◆ erase() [1/2]

iterator gtsam::FactorGraph< GaussianFactor >::erase ( iterator  item)
inlineinherited

Erase factor and rearrange other factors to take up the empty space

◆ erase() [2/2]

iterator gtsam::FactorGraph< GaussianFactor >::erase ( iterator  first,
iterator  last 
)
inlineinherited

Erase factors and rearrange other factors to take up the empty space

◆ error() [1/2]

double gtsam::GaussianFactorGraph::error ( const VectorValues x) const

unnormalized error

◆ error() [2/2]

double gtsam::FactorGraph< GaussianFactor >::error ( const HybridValues values) const
inherited

Add error for all factors.

◆ exists()

bool gtsam::FactorGraph< GaussianFactor >::exists ( size_t  idx) const
inlineinherited

MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer

◆ front()

sharedFactor gtsam::FactorGraph< GaussianFactor >::front ( ) const
inlineinherited

Get the first factor

◆ gaussianErrors()

Errors gtsam::GaussianFactorGraph::gaussianErrors ( const VectorValues x) const

return A*x-b

◆ gradient()

VectorValues gtsam::GaussianFactorGraph::gradient ( const VectorValues x0) const

Compute the gradient of the energy function, \( \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \), centered around \( x = x_0 \). The gradient is \( A^T(Ax-b) \).

Parameters
fgThe Jacobian factor graph $(A,b)$
x0The center about which to compute the gradient
Returns
The gradient as a VectorValues

◆ gradientAtZero()

virtual VectorValues gtsam::GaussianFactorGraph::gradientAtZero ( ) const
virtual

Compute the gradient of the energy function, \( \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \), centered around zero. The gradient is \( A^T(Ax-b) \).

Parameters
fgThe Jacobian factor graph $(A,b)$
[output]g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
Returns
The gradient as a VectorValues

◆ hessian() [1/2]

std::pair<Matrix,Vector> gtsam::GaussianFactorGraph::hessian ( const Ordering ordering) const

Return the dense Hessian \( \Lambda \) and information vector \( \eta \), with the noise models baked in. The negative log-likelihood is {1}{2} x^T x + ^T x + c. See also GaussianFactorGraph::augmentedHessian.

◆ hessian() [2/2]

std::pair<Matrix,Vector> gtsam::GaussianFactorGraph::hessian ( ) const

Return the dense Hessian \( \Lambda \) and information vector \( \eta \), with the noise models baked in. The negative log-likelihood is {1}{2} x^T x + ^T x + c. See also GaussianFactorGraph::augmentedHessian.

◆ hessianBlockDiagonal()

virtual std::map<Key,Matrix> gtsam::GaussianFactorGraph::hessianBlockDiagonal ( ) const
virtual

Return the block diagonal of the Hessian for this factor

◆ hessianDiagonal()

virtual VectorValues gtsam::GaussianFactorGraph::hessianDiagonal ( ) const
virtual

Return only the diagonal of the Hessian A'*A, as a VectorValues

◆ jacobian() [1/2]

std::pair<Matrix,Vector> gtsam::GaussianFactorGraph::jacobian ( const Ordering ordering) const

Return the dense Jacobian \( A \) and right-hand-side \( b \), with the noise models baked into A and b. The negative log-likelihood is \( \frac{1}{2} \Vert Ax-b \Vert^2 \). See also GaussianFactorGraph::augmentedJacobian and GaussianFactorGraph::sparseJacobian.

◆ jacobian() [2/2]

std::pair<Matrix,Vector> gtsam::GaussianFactorGraph::jacobian ( ) const

Return the dense Jacobian \( A \) and right-hand-side \( b \), with the noise models baked into A and b. The negative log-likelihood is \( \frac{1}{2} \Vert Ax-b \Vert^2 \). See also GaussianFactorGraph::augmentedJacobian and GaussianFactorGraph::sparseJacobian.

◆ keyVector()

KeyVector gtsam::FactorGraph< GaussianFactor >::keyVector ( ) const
inherited

Potentially slow function to return all keys involved, sorted, as a vector

◆ marginal()

std::shared_ptr< GaussianFactorGraph > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginal ( const KeyVector variables,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal factor graph of the requested variables.

◆ marginalMultifrontalBayesNet() [1/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesNet ( const Ordering variables,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes net. Uses COLAMD marginalization ordering by default

Parameters
variablesDetermines the ordered variables whose marginal to compute, will be ordered in the returned BayesNet as specified.
functionOptional dense elimination function.
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ marginalMultifrontalBayesNet() [2/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesNet ( const KeyVector variables,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes net. Uses COLAMD marginalization ordering by default

Parameters
variablesDetermines the variables whose marginal to compute, will be ordered using COLAMD; use Ordering(variables) to specify the variable ordering.
functionOptional dense elimination function.
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ marginalMultifrontalBayesNet() [3/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesNet ( const Ordering variables,
const Ordering marginalizedVariableOrdering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes net.

Parameters
variablesDetermines the ordered variables whose marginal to compute, will be ordered in the returned BayesNet as specified.
marginalizedVariableOrderingOrdering for the variables being marginalized out, i.e. all variables not in variables.
functionOptional dense elimination function.
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ marginalMultifrontalBayesNet() [4/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesNetType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesNet ( const KeyVector variables,
const Ordering marginalizedVariableOrdering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes net.

Parameters
variablesDetermines the variables whose marginal to compute, will be ordered using COLAMD; use Ordering(variables) to specify the variable ordering.
marginalizedVariableOrderingOrdering for the variables being marginalized out, i.e. all variables not in variables.
functionOptional dense elimination function.
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ marginalMultifrontalBayesTree() [1/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesTree ( const Ordering variables,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes tree. Uses COLAMD marginalization order by default

Parameters
variablesDetermines the ordered variables whose marginal to compute, will be ordered in the returned BayesNet as specified.
functionOptional dense elimination function..
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ marginalMultifrontalBayesTree() [2/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesTree ( const KeyVector variables,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes tree. Uses COLAMD marginalization order by default

Parameters
variablesDetermines the variables whose marginal to compute, will be ordered using COLAMD; use Ordering(variables) to specify the variable ordering.
functionOptional dense elimination function..
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ marginalMultifrontalBayesTree() [3/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesTree ( const Ordering variables,
const Ordering marginalizedVariableOrdering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes tree.

Parameters
variablesDetermines the ordered variables whose marginal to compute, will be ordered in the returned BayesNet as specified.
marginalizedVariableOrderingOrdering for the variables being marginalized out, i.e. all variables not in variables.
functionOptional dense elimination function..
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ marginalMultifrontalBayesTree() [4/4]

std::shared_ptr< typename EliminateableFactorGraph< GaussianFactorGraph >::BayesTreeType > gtsam::EliminateableFactorGraph< GaussianFactorGraph >::marginalMultifrontalBayesTree ( const KeyVector variables,
const Ordering marginalizedVariableOrdering,
const Eliminate function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex  variableIndex = {} 
) const
inherited

Compute the marginal of the requested variables and return the result as a Bayes tree.

Parameters
variablesDetermines the variables whose marginal to compute, will be ordered using COLAMD; use Ordering(variables) to specify the variable ordering.
marginalizedVariableOrderingOrdering for the variables being marginalized out, i.e. all variables not in variables.
functionOptional dense elimination function..
variableIndexOptional pre-computed VariableIndex for the factor graph, if not provided one will be computed.

◆ multiplyHessianAdd()

void gtsam::GaussianFactorGraph::multiplyHessianAdd ( double  alpha,
const VectorValues x,
VectorValues y 
) const

y += alpha*A'A*x

◆ multiplyInPlace() [1/2]

void gtsam::GaussianFactorGraph::multiplyInPlace ( const VectorValues x,
Errors e 
) const

In-place version e <- A*x that overwrites e.

◆ multiplyInPlace() [2/2]

void gtsam::GaussianFactorGraph::multiplyInPlace ( const VectorValues x,
const Errors::iterator &  e 
) const

In-place version e <- A*x that takes an iterator.

◆ negate()

GaussianFactorGraph gtsam::GaussianFactorGraph::negate ( ) const

Returns the negation of all factors in this graph - corresponds to antifactors. Will convert all factors to HessianFactors due to negation of information. Cloning preserves null factors so indices for the original graph are still valid for the cloned graph.

◆ nrFactors()

size_t gtsam::FactorGraph< GaussianFactor >::nrFactors ( ) const
inherited

return the number of non-null factors

◆ operator*()

Errors gtsam::GaussianFactorGraph::operator* ( const VectorValues x) const

return A*x

◆ operator[]() [1/2]

const sharedFactor gtsam::FactorGraph< GaussianFactor >::operator[] ( size_t  i) const
inlineinherited

Get a specific factor by index (this does not check array bounds, as opposed to at() which does).

◆ operator[]() [2/2]

sharedFactor& gtsam::FactorGraph< GaussianFactor >::operator[] ( size_t  i)
inlineinherited

Get a specific factor by index (this does not check array bounds, as opposed to at() which does).

◆ optimize() [1/2]

VectorValues gtsam::GaussianFactorGraph::optimize ( const Eliminate function = EliminationTraitsType::DefaultEliminate) const

Solve the factor graph by performing multifrontal variable elimination in COLAMD order using the dense elimination function specified in function (default EliminatePreferCholesky), followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent to calling graph.eliminateMultifrontal()->optimize().

◆ optimize() [2/2]

VectorValues gtsam::GaussianFactorGraph::optimize ( const Ordering ,
const Eliminate function = EliminationTraitsType::DefaultEliminate 
) const

Solve the factor graph by performing multifrontal variable elimination in COLAMD order using the dense elimination function specified in function (default EliminatePreferCholesky), followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent to calling graph.eliminateMultifrontal()->optimize().

◆ optimizeDensely()

VectorValues gtsam::GaussianFactorGraph::optimizeDensely ( ) const

Optimize using Eigen's dense Cholesky factorization

◆ optimizeGradientSearch()

VectorValues gtsam::GaussianFactorGraph::optimizeGradientSearch ( ) const

Optimize along the gradient direction, with a closed-form computation to perform the line search. The gradient is computed about \( \delta x=0 \).

This function returns \( \delta x \) that minimizes a reparametrized problem. The error function of a GaussianBayesNet is

\[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x + \frac{1}{2} \delta x^T R^T R \delta x \]

with gradient and Hessian

\[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \]

This function performs the line search in the direction of the gradient evaluated at \( g = g(\delta x = 0) \) with step size \( \alpha \) that minimizes \( f(\delta x = \alpha g) \):

\[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \]

Optimizing by setting the derivative to zero yields \( \hat \alpha = (-g^T g) / (g^T G g) \). For efficiency, this function evaluates the denominator without computing the Hessian \( G \), returning

\[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \]

◆ probPrime()

double gtsam::GaussianFactorGraph::probPrime ( const VectorValues c) const

Unnormalized probability. O(n)

◆ push_back() [1/4]

IsDerived<DERIVEDFACTOR> gtsam::FactorGraph< GaussianFactor >::push_back ( const DERIVEDFACTOR &  factor)
inlineinherited

Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).

◆ push_back() [2/4]

HasDerivedElementType<ITERATOR> gtsam::FactorGraph< GaussianFactor >::push_back ( ITERATOR  firstFactor,
ITERATOR  lastFactor 
)
inlineinherited

Push back many factors with an iterator over shared_ptr (factors are not copied)

◆ push_back() [3/4]

HasDerivedElementType<CONTAINER> gtsam::FactorGraph< GaussianFactor >::push_back ( const CONTAINER &  container)
inlineinherited

Push back many factors as shared_ptr's in a container (factors are not copied)

◆ push_back() [4/4]

std::enable_if< std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type gtsam::FactorGraph< GaussianFactor >::push_back ( const BayesTree< CLIQUE > &  bayesTree)
inlineinherited

Push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived classes in favor of a type-specialized version that calls this templated function.

◆ remove()

void gtsam::FactorGraph< GaussianFactor >::remove ( size_t  i)
inlineinherited

delete factor without re-arranging indexes by inserting a nullptr pointer

◆ replace()

void gtsam::FactorGraph< GaussianFactor >::replace ( size_t  index,
sharedFactor  factor 
)
inlineinherited

replace a factor by index

◆ reserve()

void gtsam::FactorGraph< GaussianFactor >::reserve ( size_t  size)
inlineinherited

Reserve space for the specified number of factors if you know in advance how many there will be (works like FastVector::reserve).

◆ resize()

virtual void gtsam::FactorGraph< GaussianFactor >::resize ( size_t  size)
inlinevirtualinherited

Directly resize the number of factors in the graph. If the new size is less than the original, factors at the end will be removed. If the new size is larger than the original, null factors will be appended.

◆ size()

size_t gtsam::FactorGraph< GaussianFactor >::size ( ) const
inlineinherited

return the number of factors (including any null factors set by remove() ).

◆ sparseJacobian() [1/2]

std::vector<std::tuple<int, int, double> > gtsam::GaussianFactorGraph::sparseJacobian ( const Ordering ordering,
size_t &  nrows,
size_t &  ncols 
) const

Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, where i(k) and j(k) are the base 0 row and column indices, and s(k) is the entry as a double. The standard deviations are baked into A and b

Returns
the sparse matrix as a std::vector of std::tuples
Parameters
orderingthe column ordering
[out]nrowsThe number of rows in the augmented Jacobian
[out]ncolsThe number of columns in the augmented Jacobian

◆ sparseJacobian() [2/2]

std::vector<std::tuple<int, int, double> > gtsam::GaussianFactorGraph::sparseJacobian ( ) const

Returns a sparse augmented Jacobian matrix with default ordering

◆ sparseJacobian_()

Matrix gtsam::GaussianFactorGraph::sparseJacobian_ ( ) const

Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. Note: i, j are 1-indexed. The standard deviations are baked into A and b

◆ transposeMultiply()

VectorValues gtsam::GaussianFactorGraph::transposeMultiply ( const Errors e) const

x = A'*e

◆ transposeMultiplyAdd()

void gtsam::GaussianFactorGraph::transposeMultiplyAdd ( double  alpha,
const Errors e,
VectorValues x 
) const

x += alpha*A'*e

Member Data Documentation

◆ factors_

FastVector<sharedFactor> gtsam::FactorGraph< GaussianFactor >::factors_
protectedinherited

concept check, makes sure FACTOR defines print and equals Collection of factors


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