Public Types | Public Member Functions | Friends | List of all members
gtsam::GaussianFactorGraph Class Reference

#include <GaussianFactorGraph.h>

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

Public Types

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

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())
 
virtual GaussianFactorGraph clone () const
 
virtual GaussianFactorGraph::shared_ptr cloneToPtr () const
 
double error (const VectorValues &x) const
 
std::map< Key, size_tgetKeyDimMap () const
 
Keys keys () const
 
GaussianFactorGraph negate () const
 
double probPrime (const VectorValues &c) 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, Vectorjacobian (const Ordering &ordering) const
 
std::pair< Matrix, Vectorjacobian () const
 
Matrix augmentedHessian (const Ordering &ordering) const
 
Matrix augmentedHessian () const
 
std::pair< Matrix, Vectorhessian (const Ordering &ordering) const
 
std::pair< Matrix, Vectorhessian () const
 
virtual VectorValues hessianDiagonal () const
 
virtual std::map< Key, MatrixhessianBlockDiagonal () 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
 
- Public Member Functions inherited from gtsam::FactorGraph< GaussianFactor >
 FactorGraph (std::initializer_list< std::shared_ptr< DERIVEDFACTOR >> sharedFactors)
 
virtual ~FactorGraph ()=default
 
void reserve (size_t size)
 
IsDerived< DERIVEDFACTOR > push_back (std::shared_ptr< DERIVEDFACTOR > factor)
 Add a factor directly using a shared_ptr. More...
 
IsDerived< DERIVEDFACTOR > push_back (const DERIVEDFACTOR &factor)
 
IsDerived< DERIVEDFACTOR > emplace_shared (Args &&... args)
 Emplace a shared pointer to factor of given type. More...
 
IsDerived< DERIVEDFACTOR > add (std::shared_ptr< DERIVEDFACTOR > factor)
 add is a synonym for push_back. More...
 
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) More...
 
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). More...
 
void add (const FACTOR_OR_CONTAINER &factorOrContainer)
 
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)
 
virtual void print (const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
 Print out graph to std::cout, with optional key formatter. More...
 
bool equals (const This &fg, double tol=1e-9) const
 Check equality up to tolerance. More...
 
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
 
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)
 
void dot (std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format, stream version. More...
 
std::string dot (const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 Output to graphviz format string. More...
 
void saveGraph (const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
 output to file with graphviz format. More...
 
size_t nrFactors () const
 
KeySet keys () const
 
KeyVector keyVector () const
 
bool exists (size_t idx) const
 
- Public Member Functions inherited from gtsam::EliminateableFactorGraph< GaussianFactorGraph >
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< 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::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::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< FactorGraphTypemarginal (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
 

Friends

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

Additional Inherited Members

- Protected Member Functions inherited from gtsam::FactorGraph< GaussianFactor >
bool isEqual (const FactorGraph &other) const
 Check exact equality of the factor pointers. Useful for derived ==. More...
 
 FactorGraph ()
 
 FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
 
 FactorGraph (const CONTAINER &factors)
 
- Protected Attributes inherited from gtsam::FactorGraph< GaussianFactor >
FastVector< sharedFactorfactors_
 

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.

Definition at line 73 of file GaussianFactorGraph.h.

Member Typedef Documentation

◆ Base

Typedef to base factor graph type.

Definition at line 80 of file GaussianFactorGraph.h.

◆ BaseEliminateable

Typedef to base elimination class.

Definition at line 81 of file GaussianFactorGraph.h.

◆ Keys

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

Definition at line 161 of file GaussianFactorGraph.h.

◆ shared_ptr

typedef std::shared_ptr<This> gtsam::GaussianFactorGraph::shared_ptr

shared_ptr to this class

Definition at line 82 of file GaussianFactorGraph.h.

◆ This

Typedef to this class.

Definition at line 79 of file GaussianFactorGraph.h.

Constructor & Destructor Documentation

◆ GaussianFactorGraph() [1/5]

gtsam::GaussianFactorGraph::GaussianFactorGraph ( )
inline

Default constructor

Definition at line 88 of file GaussianFactorGraph.h.

◆ 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 };

Definition at line 95 of file GaussianFactorGraph.h.

◆ GaussianFactorGraph() [3/5]

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

Construct from iterator over factors

Definition at line 100 of file GaussianFactorGraph.h.

◆ GaussianFactorGraph() [4/5]

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

Construct from container of factors (shared_ptr or plain objects)

Definition at line 104 of file GaussianFactorGraph.h.

◆ GaussianFactorGraph() [5/5]

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

Implicit copy/downcast constructor to override explicit template container constructor

Definition at line 108 of file GaussianFactorGraph.h.

Member Function Documentation

◆ add() [1/7]

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

Add a factor by value - makes a copy

Definition at line 125 of file GaussianFactorGraph.h.

◆ add() [2/7]

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

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

Definition at line 128 of file GaussianFactorGraph.h.

◆ add() [3/7]

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

Add a null factor

Definition at line 131 of file GaussianFactorGraph.h.

◆ add() [4/7]

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

Add a unary factor

Definition at line 135 of file GaussianFactorGraph.h.

◆ add() [5/7]

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

Definition at line 140 of file GaussianFactorGraph.h.

◆ add() [6/7]

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

Definition at line 146 of file GaussianFactorGraph.h.

◆ add() [7/7]

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

Add an n-ary factor

Definition at line 154 of file GaussianFactorGraph.h.

◆ 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 $.

Definition at line 247 of file GaussianFactorGraph.cpp.

◆ 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 $.

Definition at line 256 of file GaussianFactorGraph.cpp.

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

Definition at line 217 of file GaussianFactorGraph.cpp.

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

Definition at line 225 of file GaussianFactorGraph.cpp.

◆ clone()

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.

Definition at line 94 of file GaussianFactorGraph.cpp.

◆ cloneToPtr()

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!

Definition at line 87 of file GaussianFactorGraph.cpp.

◆ equals()

bool gtsam::GaussianFactorGraph::equals ( const This fg,
double  tol = 1e-9 
) const

Definition at line 42 of file GaussianFactorGraph.cpp.

◆ error()

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

unnormalized error

Definition at line 71 of file GaussianFactorGraph.cpp.

◆ gaussianErrors()

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

return A*x-b

Definition at line 512 of file GaussianFactorGraph.cpp.

◆ getKeyDimMap()

std::map< Key, size_t > gtsam::GaussianFactorGraph::getKeyDimMap ( ) const

Definition at line 57 of file GaussianFactorGraph.cpp.

◆ 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

Definition at line 357 of file GaussianFactorGraph.cpp.

◆ gradientAtZero()

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

Definition at line 369 of file GaussianFactorGraph.cpp.

◆ hessian() [1/2]

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.

Definition at line 264 of file GaussianFactorGraph.cpp.

◆ hessian() [2/2]

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.

Definition at line 272 of file GaussianFactorGraph.cpp.

◆ hessianBlockDiagonal()

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

Return the block diagonal of the Hessian for this factor

Definition at line 290 of file GaussianFactorGraph.cpp.

◆ hessianDiagonal()

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

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

Definition at line 279 of file GaussianFactorGraph.cpp.

◆ jacobian() [1/2]

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.

Definition at line 232 of file GaussianFactorGraph.cpp.

◆ jacobian() [2/2]

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.

Definition at line 240 of file GaussianFactorGraph.cpp.

◆ keys()

GaussianFactorGraph::Keys gtsam::GaussianFactorGraph::keys ( ) const

Definition at line 48 of file GaussianFactorGraph.cpp.

◆ multiplyHessianAdd()

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

y += alpha*A'A*x

Definition at line 420 of file GaussianFactorGraph.cpp.

◆ multiplyInPlace() [1/2]

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

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

Definition at line 427 of file GaussianFactorGraph.cpp.

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

Definition at line 432 of file GaussianFactorGraph.cpp.

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

Definition at line 106 of file GaussianFactorGraph.cpp.

◆ operator*()

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

return A*x

Definition at line 410 of file GaussianFactorGraph.cpp.

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

Definition at line 309 of file GaussianFactorGraph.cpp.

◆ optimize() [2/2]

VectorValues gtsam::GaussianFactorGraph::optimize ( const Ordering 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().

Definition at line 316 of file GaussianFactorGraph.cpp.

◆ optimizeDensely()

VectorValues gtsam::GaussianFactorGraph::optimizeDensely ( ) const

Optimize using Eigen's dense Cholesky factorization

Definition at line 323 of file GaussianFactorGraph.cpp.

◆ 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)} \]

Definition at line 381 of file GaussianFactorGraph.cpp.

◆ printErrors()

void gtsam::GaussianFactorGraph::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

Definition at line 523 of file GaussianFactorGraph.cpp.

◆ probPrime()

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

Unnormalized probability. O(n)

Definition at line 81 of file GaussianFactorGraph.cpp.

◆ sparseJacobian() [1/2]

SparseTriplets 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

Definition at line 119 of file GaussianFactorGraph.cpp.

◆ sparseJacobian() [2/2]

SparseTriplets gtsam::GaussianFactorGraph::sparseJacobian ( ) const

Returns a sparse augmented Jacobian matrix with default ordering

Definition at line 193 of file GaussianFactorGraph.cpp.

◆ 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

Definition at line 199 of file GaussianFactorGraph.cpp.

◆ transposeMultiply()

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

x = A'*e

  • ************************************************************************* */* ************************************************************************* */

Definition at line 493 of file GaussianFactorGraph.cpp.

◆ transposeMultiplyAdd()

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

x += alpha*A'*e

Definition at line 455 of file GaussianFactorGraph.cpp.

Friends And Related Function Documentation

◆ operator==

bool operator== ( const GaussianFactorGraph lhs,
const GaussianFactorGraph rhs 
)
friend

Check exact equality.

Definition at line 119 of file GaussianFactorGraph.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:46:19