Loading...
Searching...
No Matches
WhiteNoiseFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <cmath>
namespace gtsam {
const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
class WhiteNoiseFactor: public NonlinearFactor {
private:
double z_;
Key meanKey_;
Key precisionKey_;
typedef NonlinearFactor Base;
public:
static double f(double z, double u, double p) {
return logSqrt2PI - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p;
}
static HessianFactor::shared_ptr linearize(double z, double u, double p,
Key j1, Key j2) {
double e = u - z, e2 = e * e;
double c = 2 * logSqrt2PI - log(p) + e2 * p;
Vector g1 = (Vector(1) << -e * p).finished();
Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished();
Matrix G11 = (Matrix(1, 1) << p).finished();
Matrix G12 = (Matrix(1, 1) << e).finished();
Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished();
return HessianFactor::shared_ptr(
new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
}
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
}
~WhiteNoiseFactor() override {
}
void print(const std::string& p = "WhiteNoiseFactor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(p, keyFormatter);
std::cout << p + ".z: " << z_ << std::endl;
}
size_t dim() const override {
return 2;
}
double error(const Values& x) const override {
return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
}
virtual Vector unwhitenedError(const Values& x) const {
return (Vector(1) << std::sqrt(2 * error(x))).finished();
}
// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
// return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
// }
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
double u = x.at<double>(meanKey_);
double p = x.at<double>(precisionKey_);
Key j1 = meanKey_;
Key j2 = precisionKey_;
return linearize(z_, u, p, j1, j2);
}
// TODO: Frank commented this out for now, can it go?
// /// @return a deep copy of this factor
// gtsam::NonlinearFactor::shared_ptr clone() const override {
// return boost::static_pointer_cast<gtsam::NonlinearFactor>(
// gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
};
// WhiteNoiseFactor
}// namespace gtsam
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::logSqrt2PI
const double logSqrt2PI
constant needed below
Definition WhiteNoiseFactor.h:28
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form).
Definition HessianFactor.h:101
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition HessianFactor.h:110
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:58
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactor.cpp:37
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
gtsam::WhiteNoiseFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize returns a Hessianfactor that is an approximation of error(p)
Definition WhiteNoiseFactor.h:158
gtsam::WhiteNoiseFactor::~WhiteNoiseFactor
~WhiteNoiseFactor() override
Destructor.
Definition WhiteNoiseFactor.h:105
gtsam::WhiteNoiseFactor::linearize
static HessianFactor::shared_ptr linearize(double z, double u, double p, Key j1, Key j2)
linearize returns a Hessianfactor that approximates error Hessian is
Definition WhiteNoiseFactor.h:75
gtsam::WhiteNoiseFactor::print
void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition WhiteNoiseFactor.h:113
gtsam::WhiteNoiseFactor::WhiteNoiseFactor
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
Construct from measurement.
Definition WhiteNoiseFactor.h:96
gtsam::WhiteNoiseFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x) const
Vector of errors "unwhitened" does not make sense for this factor What is meant typically is only "e"...
Definition WhiteNoiseFactor.h:140
gtsam::WhiteNoiseFactor::f
static double f(double z, double u, double p)
negative log likelihood as a function of mean and precision
Definition WhiteNoiseFactor.h:61
gtsam::WhiteNoiseFactor::error
double error(const Values &x) const override
Calculate the error of the factor, typically equal to log-likelihood.
Definition WhiteNoiseFactor.h:129
gtsam::WhiteNoiseFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition WhiteNoiseFactor.h:124