#include <GaussianConditional.h>
Public Types | |
typedef Conditional< BaseFactor, This > | BaseConditional |
Typedef to our conditional base class. More... | |
typedef JacobianFactor | BaseFactor |
Typedef to our factor base class. More... | |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class More... | |
typedef GaussianConditional | This |
Typedef to this class. More... | |
Public Types inherited from gtsam::JacobianFactor | |
typedef VerticalBlockMatrix::Block | ABlock |
typedef GaussianFactor | Base |
Typedef to base class. More... | |
typedef ABlock::ColXpr | BVector |
typedef VerticalBlockMatrix::constBlock | constABlock |
typedef constABlock::ConstColXpr | constBVector |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class More... | |
typedef JacobianFactor | This |
Typedef to this class. More... | |
Public Types inherited from gtsam::GaussianFactor | |
typedef Factor | Base |
Our base class. More... | |
typedef std::shared_ptr< This > | shared_ptr |
shared_ptr to this class More... | |
typedef GaussianFactor | This |
This class. More... | |
Public Types inherited from gtsam::Factor | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. More... | |
typedef KeyVector::iterator | iterator |
Iterator over keys. More... | |
Public Types inherited from gtsam::Conditional< JacobianFactor, GaussianConditional > | |
typedef std::pair< typename JacobianFactor ::const_iterator, typename JacobianFactor ::const_iterator > | ConstFactorRange |
typedef ConstFactorRangeIterator | Frontals |
typedef ConstFactorRangeIterator | Parents |
Public Member Functions | |
Testable | |
void | print (const std::string &="GaussianConditional", const KeyFormatter &formatter=DefaultKeyFormatter) const override |
bool | equals (const GaussianFactor &cg, double tol=1e-9) const override |
Standard Interface | |
double | negLogConstant () const override |
Return the negative log of the normalization constant. More... | |
double | logProbability (const VectorValues &x) const |
double | evaluate (const VectorValues &x) const |
double | operator() (const VectorValues &x) const |
Evaluate probability density, sugar. More... | |
VectorValues | solve (const VectorValues &parents) const |
VectorValues | solveOtherRHS (const VectorValues &parents, const VectorValues &rhs) const |
void | solveTransposeInPlace (VectorValues &gy) const |
JacobianFactor::shared_ptr | likelihood (const VectorValues &frontalValues) const |
JacobianFactor::shared_ptr | likelihood (const Vector &frontal) const |
VectorValues | sample (std::mt19937_64 *rng) const |
VectorValues | sample (const VectorValues &parentsValues, std::mt19937_64 *rng) const |
VectorValues | sample () const |
Sample, use default rng. More... | |
VectorValues | sample (const VectorValues &parentsValues) const |
Sample with given values, use default rng. More... | |
Linear algebra. | |
constABlock | R () const |
constABlock | S () const |
constABlock | S (const_iterator it) const |
const constBVector | d () const |
double | determinant () const |
Compute the determinant of the R matrix. More... | |
double | logDeterminant () const |
Compute the log determinant of the R matrix. More... | |
HybridValues methods. | |
double | logProbability (const HybridValues &x) const override |
double | evaluate (const HybridValues &x) const override |
double | error (const VectorValues &c) const override |
Public Member Functions inherited from gtsam::JacobianFactor | |
Matrix | augmentedInformation () const override |
Matrix | augmentedJacobian () const override |
Matrix | augmentedJacobianUnweighted () const |
GaussianFactor::shared_ptr | clone () const override |
size_t | cols () const |
std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > | eliminate (const Ordering &keys) |
bool | equals (const GaussianFactor &lf, double tol=1e-9) const override |
assert equality up to a tolerance More... | |
double | error (const HybridValues &c) const override |
virtual double | error (const VectorValues &c) const |
double | error (const VectorValues &c) const override |
Vector | error_vector (const VectorValues &c) const |
SharedDiagonal & | get_model () |
const SharedDiagonal & | get_model () const |
ABlock | getA () |
constABlock | getA () const |
ABlock | getA (const Key &key) |
constABlock | getA (const_iterator variable) const |
ABlock | getA (iterator variable) |
BVector | getb () |
const constBVector | getb () const |
DenseIndex | getDim (const_iterator variable) const override |
Vector | gradient (Key key, const VectorValues &x) const override |
Compute the gradient wrt a key at any values. More... | |
VectorValues | gradientAtZero () const override |
A'*b for Jacobian. More... | |
void | gradientAtZero (double *d) const override |
A'*b for Jacobian (raw memory version) More... | |
std::map< Key, Matrix > | hessianBlockDiagonal () const override |
Return the block diagonal of the Hessian for this factor. More... | |
void | hessianDiagonal (double *d) const override |
Raw memory access version of hessianDiagonal. More... | |
void | hessianDiagonalAdd (VectorValues &d) const override |
Add the current diagonal to a VectorValues instance. More... | |
Matrix | information () const override |
bool | isConstrained () const |
std::pair< Matrix, Vector > | jacobian () const override |
Returns (dense) A,b pair associated with factor, bakes in the weights. More... | |
JacobianFactor () | |
JacobianFactor (const GaussianFactor &gf) | |
JacobianFactor (const GaussianFactorGraph &graph) | |
JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering) | |
JacobianFactor (const GaussianFactorGraph &graph, const Ordering &ordering, const VariableSlots &p_variableSlots) | |
JacobianFactor (const GaussianFactorGraph &graph, const VariableSlots &p_variableSlots) | |
JacobianFactor (const HessianFactor &hf) | |
JacobianFactor (const JacobianFactor &jf) | |
template<typename KEYS > | |
JacobianFactor (const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal()) | |
template<typename TERMS > | |
JacobianFactor (const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
JacobianFactor (const Vector &b_in) | |
JacobianFactor (Key i1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
JacobianFactor (Key i1, const Matrix &A1, Key i2, const Matrix &A2, Key i3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal()) | |
std::pair< Matrix, Vector > | jacobianUnweighted () const |
Returns (dense) A,b pair associated with factor, does not bake in weights. More... | |
VerticalBlockMatrix & | matrixObject () |
const VerticalBlockMatrix & | matrixObject () const |
void | multiplyHessianAdd (double alpha, const double *x, double *y, const std::vector< size_t > &accumulatedDims) const |
void | multiplyHessianAdd (double alpha, const VectorValues &x, VectorValues &y) const override |
GaussianFactor::shared_ptr | negate () const override |
Vector | operator* (const VectorValues &x) const |
void | print (const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override |
print with optional string More... | |
size_t | rows () const |
void | setModel (bool anyConstrained, const Vector &sigmas) |
std::shared_ptr< GaussianConditional > | splitConditional (size_t nrFrontals) |
void | transposeMultiplyAdd (double alpha, const Vector &e, VectorValues &x) const |
Vector | unweighted_error (const VectorValues &c) const |
void | updateHessian (const KeyVector &keys, SymmetricBlockMatrix *info) const override |
JacobianFactor | whiten () const |
~JacobianFactor () override | |
Public Member Functions inherited from gtsam::GaussianFactor | |
GaussianFactor () | |
template<typename CONTAINER > | |
GaussianFactor (const CONTAINER &keys) | |
double | error (const HybridValues &c) const override |
VectorValues | hessianDiagonal () const |
Return the diagonal of the Hessian for this factor. More... | |
Public Member Functions inherited from gtsam::Factor | |
virtual | ~Factor ()=default |
Default destructor. More... | |
bool | empty () const |
Whether the factor is empty (involves zero variables). More... | |
Key | front () const |
First key. More... | |
Key | back () const |
Last key. More... | |
const_iterator | find (Key key) const |
find More... | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. More... | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys More... | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
Public Member Functions inherited from gtsam::Conditional< JacobianFactor, GaussianConditional > | |
void | print (const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const |
bool | equals (const This &c, double tol=1e-9) const |
virtual | ~Conditional () |
size_t | nrFrontals () const |
size_t | nrParents () const |
Key | firstFrontalKey () const |
Frontals | frontals () const |
Parents | parents () const |
double | operator() (const HybridValues &x) const |
Evaluate probability density, sugar. More... | |
virtual double | negLogConstant () const |
All conditional types need to implement this as the negative log of the normalization constant to make it such that error>=0. More... | |
size_t & | nrFrontals () |
JacobianFactor ::const_iterator | beginFrontals () const |
JacobianFactor ::iterator | beginFrontals () |
JacobianFactor ::const_iterator | endFrontals () const |
JacobianFactor ::iterator | endFrontals () |
JacobianFactor ::const_iterator | beginParents () const |
JacobianFactor ::iterator | beginParents () |
JacobianFactor ::const_iterator | endParents () const |
JacobianFactor ::iterator | endParents () |
Constructors | |
GaussianConditional () | |
GaussianConditional (Key key, const Vector &d, const Matrix &R, const SharedDiagonal &sigmas=SharedDiagonal()) | |
GaussianConditional (Key key, const Vector &d, const Matrix &R, Key parent1, const Matrix &S, const SharedDiagonal &sigmas=SharedDiagonal()) | |
GaussianConditional (Key key, const Vector &d, const Matrix &R, Key parent1, const Matrix &S, Key parent2, const Matrix &T, const SharedDiagonal &sigmas=SharedDiagonal()) | |
template<typename TERMS > | |
GaussianConditional (const TERMS &terms, size_t nrFrontals, const Vector &d, const SharedDiagonal &sigmas=SharedDiagonal()) | |
template<typename KEYS > | |
GaussianConditional (const KEYS &keys, size_t nrFrontals, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal()) | |
static GaussianConditional | FromMeanAndStddev (Key key, const Vector &mu, double sigma) |
Construct from mean mu and standard deviation sigma . More... | |
static GaussianConditional | FromMeanAndStddev (Key key, const Matrix &A, Key parent, const Vector &b, double sigma) |
Construct from conditional mean A1 p1 + b and standard deviation. More... | |
static GaussianConditional | FromMeanAndStddev (Key key, const Matrix &A1, Key parent1, const Matrix &A2, Key parent2, const Vector &b, double sigma) |
template<typename... Args> | |
static shared_ptr | sharedMeanAndStddev (Args &&... args) |
Create shared pointer by forwarding arguments to fromMeanAndStddev. More... | |
template<typename ITERATOR > | |
static shared_ptr | Combine (ITERATOR firstConditional, ITERATOR lastConditional) |
Additional Inherited Members | |
Static Public Member Functions inherited from gtsam::GaussianFactor | |
template<typename CONTAINER > | |
static DenseIndex | Slot (const CONTAINER &keys, Key key) |
Static Public Member Functions inherited from gtsam::Conditional< JacobianFactor, GaussianConditional > | |
static bool | CheckInvariants (const GaussianConditional &conditional, const VALUES &x) |
Protected Member Functions inherited from gtsam::JacobianFactor | |
template<typename TERMS > | |
void | fillTerms (const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel) |
Internal function to fill blocks and set dimensions. More... | |
Protected Member Functions inherited from gtsam::Factor | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
Protected Member Functions inherited from gtsam::Conditional< JacobianFactor, GaussianConditional > | |
Conditional () | |
Conditional (size_t nrFrontals) | |
Static Protected Member Functions inherited from gtsam::Factor | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
Protected Attributes inherited from gtsam::JacobianFactor | |
VerticalBlockMatrix | Ab_ |
noiseModel::Diagonal::shared_ptr | model_ |
Protected Attributes inherited from gtsam::Factor | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
Protected Attributes inherited from gtsam::Conditional< JacobianFactor, GaussianConditional > | |
size_t | nrFrontals_ |
A GaussianConditional functions as the node in a Bayes network. It has a set of parents y,z, etc. and implements a Gaussian probability density p(x | y, z) on x. The negative log-density is given by The mean of the conditional density is . The covariance of the conditional density is given by the noise model and is constrained to be diagonal.
This is the base class for all conditional distributions/densities, which are implemented as specialized factors. This class does not store any data other than its keys. Derived classes store data such as matrices and probability tables.
The evaluate
method is used to evaluate the factor, and together with logProbability
is the main methods that need to be implemented in derived classes. These two methods relate to the error
method in the factor by: probability(x) = k exp(-error(x)) where k is a normalization constant making \int probability(x) = \int k exp(-error(x)) == 1.0, and logProbability(x) = -(K + error(x)) i.e., K = -log(k). The normalization constant k is assumed to not depend on any argument, only (possibly) on the conditional parameters. This class provides a default negative log normalization constant negLogConstant() == 0.0.
There are four broad classes of conditionals that derive from Conditional:
Definition at line 40 of file GaussianConditional.h.
Typedef to our conditional base class.
Definition at line 48 of file GaussianConditional.h.
Typedef to our factor base class.
Definition at line 47 of file GaussianConditional.h.
typedef std::shared_ptr<This> gtsam::GaussianConditional::shared_ptr |
shared_ptr to this class
Definition at line 46 of file GaussianConditional.h.
Typedef to this class.
Definition at line 45 of file GaussianConditional.h.
|
inline |
default constructor needed for serialization
Definition at line 54 of file GaussianConditional.h.
GaussianConditional::GaussianConditional | ( | Key | key, |
const Vector & | d, | ||
const Matrix & | R, | ||
const SharedDiagonal & | sigmas = SharedDiagonal() |
||
) |
constructor with no parents |Rx-d|
Definition at line 45 of file GaussianConditional.cpp.
GaussianConditional::GaussianConditional | ( | Key | key, |
const Vector & | d, | ||
const Matrix & | R, | ||
Key | parent1, | ||
const Matrix & | S, | ||
const SharedDiagonal & | sigmas = SharedDiagonal() |
||
) |
constructor with only one parent |Rx+Sy-d|
Definition at line 50 of file GaussianConditional.cpp.
GaussianConditional::GaussianConditional | ( | Key | key, |
const Vector & | d, | ||
const Matrix & | R, | ||
Key | parent1, | ||
const Matrix & | S, | ||
Key | parent2, | ||
const Matrix & | T, | ||
const SharedDiagonal & | sigmas = SharedDiagonal() |
||
) |
constructor with two parents |Rx+Sy+Tz-d|
Definition at line 57 of file GaussianConditional.cpp.
GaussianConditional::GaussianConditional | ( | const TERMS & | terms, |
size_t | nrFrontals, | ||
const Vector & | d, | ||
const SharedDiagonal & | sigmas = SharedDiagonal() |
||
) |
Constructor with arbitrary number of frontals and parents.
TERMS | A container whose value type is std::pair<Key, Matrix>, specifying the collection of keys and matrices making up the conditional. |
Definition at line 26 of file GaussianConditional-inl.h.
GaussianConditional::GaussianConditional | ( | const KEYS & | keys, |
size_t | nrFrontals, | ||
const VerticalBlockMatrix & | augmentedMatrix, | ||
const SharedDiagonal & | sigmas = SharedDiagonal() |
||
) |
Constructor with arbitrary number keys, and where the augmented matrix is given all together instead of in block terms. Note that only the active view of the provided augmented matrix is used, and that the matrix data is copied into a newly-allocated matrix in the constructed factor.
Definition at line 32 of file GaussianConditional-inl.h.
|
static |
Combine several GaussianConditional into a single dense GC. The conditionals enumerated by first
and last
must be in increasing order, meaning that the parents of any conditional may not include a conditional coming before it.
|
inline |
Get a view of the r.h.s. vector d
Definition at line 231 of file GaussianConditional.h.
|
inline |
Compute the determinant of the R matrix.
The determinant is computed in log form using logDeterminant for numerical stability and then exponentiated.
Note, the covariance matrix , and hence .
Definition at line 244 of file GaussianConditional.h.
|
overridevirtual |
equals function
Implements gtsam::GaussianFactor.
Definition at line 132 of file GaussianConditional.cpp.
|
override |
Definition at line 486 of file JacobianFactor.cpp.
|
overridevirtual |
Calculate probability for HybridValues x
. Simply dispatches to VectorValues version.
Reimplemented from gtsam::Conditional< JacobianFactor, GaussianConditional >.
Definition at line 213 of file GaussianConditional.cpp.
double GaussianConditional::evaluate | ( | const VectorValues & | x | ) | const |
Calculate probability density for given values x
: exp(logProbability(x)) == exp(-GaussianFactor::error(x)) / sqrt((2*pi)^n*det(Sigma)) where x is the vector of values, and Sigma is the covariance matrix.
Definition at line 209 of file GaussianConditional.cpp.
|
static |
Construct from conditional mean A1 p1 + b
and standard deviation.
Definition at line 77 of file GaussianConditional.cpp.
|
static |
Construct from conditional mean A1 p1 + A2 p2 + b
and standard deviation sigma
.
Definition at line 88 of file GaussianConditional.cpp.
|
static |
Construct from mean mu
and standard deviation sigma
.
Definition at line 66 of file GaussianConditional.cpp.
JacobianFactor::shared_ptr GaussianConditional::likelihood | ( | const Vector & | frontal | ) | const |
Single variable version of likelihood.
Definition at line 328 of file GaussianConditional.cpp.
JacobianFactor::shared_ptr GaussianConditional::likelihood | ( | const VectorValues & | frontalValues | ) | const |
Convert to a likelihood factor by providing value before bar.
Definition at line 296 of file GaussianConditional.cpp.
double GaussianConditional::logDeterminant | ( | ) | const |
Compute the log determinant of the R matrix.
For numerical stability, the determinant is computed in log form, so it is a summation rather than a multiplication.
Note, the covariance matrix , and hence .
Definition at line 172 of file GaussianConditional.cpp.
|
overridevirtual |
Calculate log-probability log(evaluate(x)) for HybridValues x
. Simply dispatches to VectorValues version.
Reimplemented from gtsam::Conditional< JacobianFactor, GaussianConditional >.
Definition at line 204 of file GaussianConditional.cpp.
double GaussianConditional::logProbability | ( | const VectorValues & | x | ) | const |
Calculate log-probability log(evaluate(x)) for given values x
: -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) where x is the vector of values, and Sigma is the covariance matrix. This differs from error as it is log, not negative log, and it includes the normalization constant.
Definition at line 200 of file GaussianConditional.cpp.
|
override |
Return the negative log of the normalization constant.
normalization constant k = 1.0 / sqrt((2*pi)^n*det(Sigma)) -log(k) = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma)
Definition at line 185 of file GaussianConditional.cpp.
|
inline |
Evaluate probability density, sugar.
Definition at line 162 of file GaussianConditional.h.
|
overridevirtual |
Implements gtsam::GaussianFactor.
Reimplemented in gtsam::GaussianDensity.
Definition at line 101 of file GaussianConditional.cpp.
|
inline |
Return a view of the upper-triangular R block of the conditional
Definition at line 222 of file GaussianConditional.h.
|
inline |
Get a view of the parent blocks.
Definition at line 225 of file GaussianConditional.h.
|
inline |
Get a view of the S matrix for the variable pointed to by the given key iterator
Definition at line 228 of file GaussianConditional.h.
VectorValues GaussianConditional::sample | ( | ) | const |
Sample, use default rng.
Definition at line 366 of file GaussianConditional.cpp.
VectorValues GaussianConditional::sample | ( | const VectorValues & | parentsValues | ) | const |
Sample with given values, use default rng.
Definition at line 370 of file GaussianConditional.cpp.
VectorValues GaussianConditional::sample | ( | const VectorValues & | parentsValues, |
std::mt19937_64 * | rng | ||
) | const |
Sample from conditional, given missing variables Example: std::mt19937_64 rng(42); VectorValues given = ...; auto sample = gbn.sample(given, &rng);
Definition at line 340 of file GaussianConditional.cpp.
VectorValues GaussianConditional::sample | ( | std::mt19937_64 * | rng | ) | const |
Sample from conditional, zero parent version Example: std::mt19937_64 rng(42); auto sample = gbn.sample(&rng);
Definition at line 357 of file GaussianConditional.cpp.
|
inlinestatic |
Create shared pointer by forwarding arguments to fromMeanAndStddev.
Definition at line 105 of file GaussianConditional.h.
VectorValues GaussianConditional::solve | ( | const VectorValues & | parents | ) | const |
Solves a conditional Gaussian and writes the solution into the entries of x
for each frontal variable of the conditional. The parents are assumed to have already been solved in and their values are read from x
. This function works for multiple frontal variables.
Given the Gaussian conditional with log likelihood , where are the frontal variables and are the separator variables of this conditional, this solve function computes using back-substitution.
parents | VectorValues containing solved parents . |
Definition at line 218 of file GaussianConditional.cpp.
VectorValues GaussianConditional::solveOtherRHS | ( | const VectorValues & | parents, |
const VectorValues & | rhs | ||
) | const |
Definition at line 245 of file GaussianConditional.cpp.
void GaussianConditional::solveTransposeInPlace | ( | VectorValues & | gy | ) | const |
Performs transpose backsubstition in place on values
Definition at line 273 of file GaussianConditional.cpp.