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

#include <GaussianBayesNet.h>

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

Public Types

typedef FactorGraph< GaussianConditionalBase
 
typedef GaussianConditional ConditionalType
 
typedef boost::shared_ptr< Thisshared_ptr
 
typedef boost::shared_ptr< ConditionalTypesharedConditional
 
typedef GaussianBayesNet This
 
- Public Types inherited from gtsam::FactorGraph< GaussianConditional >
typedef FastVector< sharedFactor >::const_iterator const_iterator
 
typedef GaussianConditional FactorType
 factor type More...
 
typedef FastVector< sharedFactor >::iterator iterator
 
typedef boost::shared_ptr< GaussianConditionalsharedFactor
 Shared pointer to a factor. More...
 
typedef sharedFactor value_type
 

Public Member Functions

Standard Constructors
 GaussianBayesNet ()
 
template<typename ITERATOR >
 GaussianBayesNet (ITERATOR firstConditional, ITERATOR lastConditional)
 
template<class CONTAINER >
 GaussianBayesNet (const CONTAINER &conditionals)
 
template<class DERIVEDCONDITIONAL >
 GaussianBayesNet (const FactorGraph< DERIVEDCONDITIONAL > &graph)
 
virtual ~GaussianBayesNet ()
 Destructor. More...
 
Testable
bool equals (const This &bn, double tol=1e-9) const
 
Standard Interface
VectorValues optimize () const
 Solve the GaussianBayesNet, i.e. return $ x = R^{-1}*d $, by back-substitution. More...
 
VectorValues optimize (const VectorValues &solutionForMissing) const
 Version of optimize for incomplete BayesNet, needs solution for missing variables. More...
 
Ordering ordering () const
 
Linear Algebra
std::pair< Matrix, Vectormatrix (const Ordering &ordering) const
 
std::pair< Matrix, Vectormatrix () const
 
VectorValues optimizeGradientSearch () const
 
VectorValues gradient (const VectorValues &x0) const
 
VectorValues gradientAtZero () const
 
double error (const VectorValues &x) const
 
double determinant () const
 
double logDeterminant () const
 
VectorValues backSubstitute (const VectorValues &gx) const
 
VectorValues backSubstituteTranspose (const VectorValues &gx) const
 
void print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
 print graph More...
 
void saveGraph (const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
 Save the GaussianBayesNet as an image. Requires dot to be installed. More...
 
- Public Member Functions inherited from gtsam::FactorGraph< GaussianConditional >
virtual ~FactorGraph ()=default
 Default destructor. More...
 
void reserve (size_t size)
 
IsDerived< DERIVEDFACTOR > push_back (boost::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 (boost::shared_ptr< DERIVEDFACTOR > factor)
 add is a synonym for push_back. More...
 
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost::assign::list_inserter< RefCallPushBack< This > > >::type operator+= (boost::shared_ptr< DERIVEDFACTOR > factor)
 += works well with boost::assign list inserter. 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)
 
boost::assign::list_inserter< CRefCallPushBack< This > > operator+= (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)
 
bool equals (const This &fg, double tol=1e-9) const
 
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
 
iterator begin ()
 
iterator end ()
 
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)
 
size_t nrFactors () const
 
KeySet keys () const
 
KeyVector keyVector () const
 
bool exists (size_t idx) const
 

Private Member Functions

template<class ARCHIVE >
void serialize (ARCHIVE &ar, const unsigned int)
 

Friends

class boost::serialization::access
 

Additional Inherited Members

- Protected Member Functions inherited from gtsam::FactorGraph< GaussianConditional >
 FactorGraph ()
 
 FactorGraph (ITERATOR firstFactor, ITERATOR lastFactor)
 
 FactorGraph (const CONTAINER &factors)
 
- Protected Attributes inherited from gtsam::FactorGraph< GaussianConditional >
FastVector< sharedFactorfactors_
 

Detailed Description

A Bayes net made from linear-Gaussian densities

Definition at line 30 of file GaussianBayesNet.h.

Member Typedef Documentation

Definition at line 34 of file GaussianBayesNet.h.

Definition at line 36 of file GaussianBayesNet.h.

typedef boost::shared_ptr<This> gtsam::GaussianBayesNet::shared_ptr

Definition at line 37 of file GaussianBayesNet.h.

Definition at line 38 of file GaussianBayesNet.h.

Definition at line 35 of file GaussianBayesNet.h.

Constructor & Destructor Documentation

gtsam::GaussianBayesNet::GaussianBayesNet ( )
inline

Construct empty factor graph

Definition at line 44 of file GaussianBayesNet.h.

template<typename ITERATOR >
gtsam::GaussianBayesNet::GaussianBayesNet ( ITERATOR  firstConditional,
ITERATOR  lastConditional 
)
inline

Construct from iterator over conditionals

Definition at line 48 of file GaussianBayesNet.h.

template<class CONTAINER >
gtsam::GaussianBayesNet::GaussianBayesNet ( const CONTAINER &  conditionals)
inlineexplicit

Construct from container of factors (shared_ptr or plain objects)

Definition at line 52 of file GaussianBayesNet.h.

template<class DERIVEDCONDITIONAL >
gtsam::GaussianBayesNet::GaussianBayesNet ( const FactorGraph< DERIVEDCONDITIONAL > &  graph)
inline

Implicit copy/downcast constructor to override explicit template container constructor

Definition at line 56 of file GaussianBayesNet.h.

virtual gtsam::GaussianBayesNet::~GaussianBayesNet ( )
inlinevirtual

Destructor.

Definition at line 59 of file GaussianBayesNet.h.

Member Function Documentation

VectorValues gtsam::GaussianBayesNet::backSubstitute ( const VectorValues gx) const

Backsubstitute with a different RHS vector than the one stored in this BayesNet. gy=inv(R*inv(Sigma))*gx

Definition at line 84 of file GaussianBayesNet.cpp.

VectorValues gtsam::GaussianBayesNet::backSubstituteTranspose ( const VectorValues gx) const

Transpose backsubstitute with a different RHS vector than the one stored in this BayesNet. gy=inv(L)*gx by solving L*gy=gx. gy=inv(R'*inv(Sigma))*gx gz'*R'=gx', gy = gz.*sigmas

Definition at line 100 of file GaussianBayesNet.cpp.

double gtsam::GaussianBayesNet::determinant ( ) const

Computes the determinant of a GassianBayesNet. A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix determinant is the product of the diagonal elements. Instead of actually multiplying we add the logarithms of the diagonal elements and take the exponent at the end because this is more numerically stable.

Parameters
bayesNetThe input GaussianBayesNet
Returns
The determinant
  • ************************************************************************* */* ************************************************************************* */

Definition at line 186 of file GaussianBayesNet.cpp.

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

Check equality

Definition at line 35 of file GaussianBayesNet.cpp.

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

0.5 * sum of squared Mahalanobis distances.

Definition at line 79 of file GaussianBayesNet.cpp.

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

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

Parameters
x0The center about which to compute the gradient
Returns
The gradient as a VectorValues

Definition at line 69 of file GaussianBayesNet.cpp.

VectorValues gtsam::GaussianBayesNet::gradientAtZero ( ) const

Compute the gradient of the energy function, $ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d \right\Vert^2 $, centered around zero. The gradient about zero is $ -R^T d $. See also gradient(const GaussianBayesNet&, const VectorValues&).

Parameters
[output]g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues

Definition at line 74 of file GaussianBayesNet.cpp.

double gtsam::GaussianBayesNet::logDeterminant ( ) const

Computes the log of the determinant of a GassianBayesNet. A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix determinant is the product of the diagonal elements.

Parameters
bayesNetThe input GaussianBayesNet
Returns
The determinant

Definition at line 192 of file GaussianBayesNet.cpp.

pair< Matrix, Vector > gtsam::GaussianBayesNet::matrix ( const Ordering ordering) const

Return (dense) upper-triangular matrix representation Will return upper-triangular matrix only when using 'ordering' above. In case Bayes net is incomplete zero columns are added to the end.

Definition at line 160 of file GaussianBayesNet.cpp.

pair< Matrix, Vector > gtsam::GaussianBayesNet::matrix ( ) const

Return (dense) upper-triangular matrix representation Will return upper-triangular matrix only when using 'ordering' above. In case Bayes net is incomplete zero columns are added to the end.

Definition at line 167 of file GaussianBayesNet.cpp.

VectorValues gtsam::GaussianBayesNet::optimize ( ) const

Solve the GaussianBayesNet, i.e. return $ x = R^{-1}*d $, by back-substitution.

Definition at line 41 of file GaussianBayesNet.cpp.

VectorValues gtsam::GaussianBayesNet::optimize ( const VectorValues solutionForMissing) const

Version of optimize for incomplete BayesNet, needs solution for missing variables.

solve each node in turn in topological sort order (parents first)

Definition at line 48 of file GaussianBayesNet.cpp.

VectorValues gtsam::GaussianBayesNet::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 62 of file GaussianBayesNet.cpp.

Ordering gtsam::GaussianBayesNet::ordering ( ) const

Return ordering corresponding to a topological sort. There are many topological sorts of a Bayes net. This one corresponds to the one that makes 'matrix' below upper-triangular. In case Bayes net is incomplete any non-frontal are added to the end.

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

Definition at line 142 of file GaussianBayesNet.cpp.

void gtsam::GaussianBayesNet::print ( const std::string &  s = "",
const KeyFormatter formatter = DefaultKeyFormatter 
) const
inlineoverridevirtual

print graph

Reimplemented from gtsam::FactorGraph< GaussianConditional >.

Definition at line 184 of file GaussianBayesNet.h.

void gtsam::GaussianBayesNet::saveGraph ( const std::string &  s,
const KeyFormatter keyFormatter = DefaultKeyFormatter 
) const

Save the GaussianBayesNet as an image. Requires dot to be installed.

Parameters
sThe name of the figure.
keyFormatterFormatter to use for styling keys in the graph.

Definition at line 208 of file GaussianBayesNet.cpp.

template<class ARCHIVE >
void gtsam::GaussianBayesNet::serialize ( ARCHIVE &  ar,
const unsigned  int 
)
inlineprivate

Definition at line 206 of file GaussianBayesNet.h.

Friends And Related Function Documentation

friend class boost::serialization::access
friend

Serialization function

Definition at line 204 of file GaussianBayesNet.h.


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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:10