gtsam 4.2.0
gtsam
WhiteNoiseFactor.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
20#pragma once
21
24#include <cmath>
25
26namespace gtsam {
27
28 const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
29
42
43 private:
44
45 double z_;
46
47 Key meanKey_;
48 Key precisionKey_;
49
50 typedef NonlinearFactor Base;
51
52 public:
53
61 static double f(double z, double u, double p) {
62 return logSqrt2PI - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p;
63 }
64
75 static HessianFactor::shared_ptr linearize(double z, double u, double p,
76 Key j1, Key j2) {
77 double e = u - z, e2 = e * e;
78 double c = 2 * logSqrt2PI - log(p) + e2 * p;
79 Vector g1 = (Vector(1) << -e * p).finished();
80 Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished();
81 Matrix G11 = (Matrix(1, 1) << p).finished();
82 Matrix G12 = (Matrix(1, 1) << e).finished();
83 Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished();
85 new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
86 }
87
90
96 WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
97 Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
98 }
99
103
105 ~WhiteNoiseFactor() override {
106 }
107
111
113 void print(const std::string& p = "WhiteNoiseFactor",
114 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
115 Base::print(p, keyFormatter);
116 std::cout << p + ".z: " << z_ << std::endl;
117 }
118
122
124 size_t dim() const override {
125 return 2;
126 }
127
129 double error(const Values& x) const override {
130 return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
131 }
132
140 virtual Vector unwhitenedError(const Values& x) const {
141 return (Vector(1) << std::sqrt(2 * error(x))).finished();
142 }
143
148// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
149// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
150// return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
151// }
152
156
158 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
159 double u = x.at<double>(meanKey_);
160 double p = x.at<double>(precisionKey_);
161 Key j1 = meanKey_;
162 Key j2 = precisionKey_;
163 return linearize(z_, u, p, j1, j2);
164 }
165
166 // TODO: Frank commented this out for now, can it go?
167 // /// @return a deep copy of this factor
168 // gtsam::NonlinearFactor::shared_ptr clone() const override {
169 // return boost::static_pointer_cast<gtsam::NonlinearFactor>(
170 // gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
171
173
174 };
175// WhiteNoiseFactor
176
177}// namespace gtsam
178
Contains the HessianFactor class, a general quadratic factor.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
const double logSqrt2PI
constant needed below
Definition: WhiteNoiseFactor.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
Definition: Factor.h:68
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:110
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: NonlinearFactor.cpp:37
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:361
Binary factor to estimate parameters of zero-mean Gaussian white noise.
Definition: WhiteNoiseFactor.h:41
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
~WhiteNoiseFactor() override
Destructor.
Definition: WhiteNoiseFactor.h:105
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
void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: WhiteNoiseFactor.h:113
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
Construct from measurement.
Definition: WhiteNoiseFactor.h:96
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
static double f(double z, double u, double p)
negative log likelihood as a function of mean and precision
Definition: WhiteNoiseFactor.h:61
double error(const Values &x) const override
Calculate the error of the factor, typically equal to log-likelihood.
Definition: WhiteNoiseFactor.h:129
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: WhiteNoiseFactor.h:124