Static Public Member Functions | Private Types | Private Attributes | List of all members
gtsam::WhiteNoiseFactor Class Reference

Binary factor to estimate parameters of zero-mean Gaussian white noise. More...

#include <WhiteNoiseFactor.h>

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

Static Public Member Functions

static double f (double z, double u, double p)
 negative log likelihood as a function of mean $ \mu $ and precision $ \tau $

\[ f(z, \tau, \mu) = -\log \left( \frac{\sqrt{\tau}}{\sqrt{2\pi}} \exp(-0.5\tau(z-\mu)^2) \right) = \log(\sqrt{2\pi}) - 0.5* \log(\tau) + 0.5\tau(z-\mu)^2 \]

More...
 
static HessianFactor::shared_ptr linearize (double z, double u, double p, Key j1, Key j2)
 linearize returns a Hessianfactor that approximates error Hessian is

\[ 0.5f - x^T g + 0.5*x^T G x \]

Taylor expansion is

\[ f(x+dx) = f(x) + df(x) dx + 0.5 ddf(x) dx^2 \]

So f = 2 f(x), g = -df(x), G = ddf(x) More...

 

Private Types

typedef NonlinearFactor Base
 

Private Attributes

Key meanKey_
 key by which to access mean variable More...
 
Key precisionKey_
 key by which to access precision variable More...
 
double z_
 Measurement. More...
 

Standard Constructors

 WhiteNoiseFactor (double z, Key meanKey, Key precisionKey)
 

Advanced Constructors

 ~WhiteNoiseFactor () override
 Destructor. More...
 

Testable

void print (const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
 Print. More...
 

Standard Interface

size_t dim () const override
 get the dimension of the factor (number of rows on linearization) More...
 
double error (const Values &x) const override
 Calculate the error of the factor, typically equal to log-likelihood. More...
 
virtual Vector unwhitenedError (const Values &x) const
 

Advanced Interface

std::shared_ptr< GaussianFactorlinearize (const Values &x) const override
 linearize returns a Hessianfactor that is an approximation of error(p) More...
 

Additional Inherited Members

- Public Types inherited from gtsam::NonlinearFactor
typedef std::shared_ptr< Thisshared_ptr
 
- 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 Member Functions inherited from gtsam::NonlinearFactor
 NonlinearFactor ()
 
template<typename CONTAINER >
 NonlinearFactor (const CONTAINER &keys)
 
virtual bool equals (const NonlinearFactor &f, double tol=1e-9) const
 
double error (const HybridValues &c) const override
 
virtual bool active (const Values &) const
 
virtual shared_ptr clone () const
 
virtual shared_ptr rekey (const std::map< Key, Key > &rekey_mapping) const
 
virtual shared_ptr rekey (const KeyVector &new_keys) const
 
virtual bool sendable () const
 
- 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 KeyVectorkeys () 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...
 
KeyVectorkeys ()
 
iterator begin ()
 
iterator end ()
 
- Protected Types inherited from gtsam::NonlinearFactor
typedef Factor Base
 
typedef NonlinearFactor This
 
- Protected Member Functions inherited from gtsam::Factor
 Factor ()
 
template<typename CONTAINER >
 Factor (const CONTAINER &keys)
 
template<typename ITERATOR >
 Factor (ITERATOR first, ITERATOR last)
 
- 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::Factor
KeyVector keys_
 The keys involved in this factor. More...
 

Detailed Description

Binary factor to estimate parameters of zero-mean Gaussian white noise.

This factor uses the mean-precision parameterization.

Takes three template arguments: Key: key type to use for mean Key: key type to use for precision Values: Values type for optimization

Definition at line 41 of file WhiteNoiseFactor.h.

Member Typedef Documentation

◆ Base

Definition at line 50 of file WhiteNoiseFactor.h.

Constructor & Destructor Documentation

◆ WhiteNoiseFactor()

gtsam::WhiteNoiseFactor::WhiteNoiseFactor ( double  z,
Key  meanKey,
Key  precisionKey 
)
inline

Construct from measurement

Parameters
zMeasurment value
meanKeyKey for mean variable
precisionKeyKey for precision variable

Definition at line 96 of file WhiteNoiseFactor.h.

◆ ~WhiteNoiseFactor()

gtsam::WhiteNoiseFactor::~WhiteNoiseFactor ( )
inlineoverride

Destructor.

Definition at line 105 of file WhiteNoiseFactor.h.

Member Function Documentation

◆ dim()

size_t gtsam::WhiteNoiseFactor::dim ( ) const
inlineoverridevirtual

get the dimension of the factor (number of rows on linearization)

Implements gtsam::NonlinearFactor.

Definition at line 124 of file WhiteNoiseFactor.h.

◆ error()

double gtsam::WhiteNoiseFactor::error ( const Values x) const
inlineoverridevirtual

Calculate the error of the factor, typically equal to log-likelihood.

Reimplemented from gtsam::NonlinearFactor.

Definition at line 129 of file WhiteNoiseFactor.h.

◆ f()

static double gtsam::WhiteNoiseFactor::f ( double  z,
double  u,
double  p 
)
inlinestatic

negative log likelihood as a function of mean $ \mu $ and precision $ \tau $

\[ f(z, \tau, \mu) = -\log \left( \frac{\sqrt{\tau}}{\sqrt{2\pi}} \exp(-0.5\tau(z-\mu)^2) \right) = \log(\sqrt{2\pi}) - 0.5* \log(\tau) + 0.5\tau(z-\mu)^2 \]

Definition at line 61 of file WhiteNoiseFactor.h.

◆ linearize() [1/2]

static HessianFactor::shared_ptr gtsam::WhiteNoiseFactor::linearize ( double  z,
double  u,
double  p,
Key  j1,
Key  j2 
)
inlinestatic

linearize returns a Hessianfactor that approximates error Hessian is

\[ 0.5f - x^T g + 0.5*x^T G x \]

Taylor expansion is

\[ f(x+dx) = f(x) + df(x) dx + 0.5 ddf(x) dx^2 \]

So f = 2 f(x), g = -df(x), G = ddf(x)

Definition at line 75 of file WhiteNoiseFactor.h.

◆ linearize() [2/2]

std::shared_ptr<GaussianFactor> gtsam::WhiteNoiseFactor::linearize ( const Values x) const
inlineoverridevirtual

linearize returns a Hessianfactor that is an approximation of error(p)

Implements gtsam::NonlinearFactor.

Definition at line 158 of file WhiteNoiseFactor.h.

◆ print()

void gtsam::WhiteNoiseFactor::print ( const std::string &  p = "WhiteNoiseFactor",
const KeyFormatter keyFormatter = DefaultKeyFormatter 
) const
inlineoverridevirtual

Print.

Reimplemented from gtsam::NonlinearFactor.

Definition at line 113 of file WhiteNoiseFactor.h.

◆ unwhitenedError()

virtual Vector gtsam::WhiteNoiseFactor::unwhitenedError ( const Values x) const
inlinevirtual

Vector of errors "unwhitened" does not make sense for this factor What is meant typically is only "e" above Here we shoehorn sqrt(2*error(p)) TODO: Where is this used? should disappear.

Definition at line 140 of file WhiteNoiseFactor.h.

Member Data Documentation

◆ meanKey_

Key gtsam::WhiteNoiseFactor::meanKey_
private

key by which to access mean variable

Definition at line 47 of file WhiteNoiseFactor.h.

◆ precisionKey_

Key gtsam::WhiteNoiseFactor::precisionKey_
private

key by which to access precision variable

Definition at line 48 of file WhiteNoiseFactor.h.

◆ z_

double gtsam::WhiteNoiseFactor::z_
private

Measurement.

Definition at line 45 of file WhiteNoiseFactor.h.


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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:15