gtsam  4.1.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 
22 #include <cmath>
23 
24 namespace gtsam {
25 
26  const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
27 
40 
41  private:
42 
43  double z_;
44 
45  Key meanKey_;
46  Key precisionKey_;
47 
48  typedef NonlinearFactor Base;
49 
50  public:
51 
59  static double f(double z, double u, double p) {
60  return logSqrt2PI - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p;
61  }
62 
73  static HessianFactor::shared_ptr linearize(double z, double u, double p,
74  Key j1, Key j2) {
75  double e = u - z, e2 = e * e;
76  double c = 2 * logSqrt2PI - log(p) + e2 * p;
77  Vector g1 = (Vector(1) << -e * p).finished();
78  Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished();
79  Matrix G11 = (Matrix(1, 1) << p).finished();
80  Matrix G12 = (Matrix(1, 1) << e).finished();
81  Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished();
83  new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
84  }
85 
88 
94  WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
95  Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
96  }
97 
101 
103  virtual ~WhiteNoiseFactor() {
104  }
105 
109 
111  void print(const std::string& p = "WhiteNoiseFactor",
112  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
113  Base::print(p, keyFormatter);
114  std::cout << p + ".z: " << z_ << std::endl;
115  }
116 
120 
122  size_t dim() const override {
123  return 2;
124  }
125 
127  double error(const Values& x) const override {
128  return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
129  }
130 
138  virtual Vector unwhitenedError(const Values& x) const {
139  return (Vector(1) << std::sqrt(2 * error(x))).finished();
140  }
141 
146 // virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
147 // const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
148 // return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
149 // }
150 
154 
156  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
157  double u = x.at<double>(meanKey_);
158  double p = x.at<double>(precisionKey_);
159  Key j1 = meanKey_;
160  Key j2 = precisionKey_;
161  return linearize(z_, u, p, j1, j2);
162  }
163 
164  // TODO: Frank commented this out for now, can it go?
165  // /// @return a deep copy of this factor
166  // gtsam::NonlinearFactor::shared_ptr clone() const override {
167  // return boost::static_pointer_cast<gtsam::NonlinearFactor>(
168  // gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
169 
171 
172  };
173 // WhiteNoiseFactor
174 
175 }// namespace gtsam
176 
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:156
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:110
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:138
gtsam::NonlinearFactor::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: NonlinearFactor.cpp:26
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
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:73
gtsam::WhiteNoiseFactor::~WhiteNoiseFactor
virtual ~WhiteNoiseFactor()
Destructor.
Definition: WhiteNoiseFactor.h:103
gtsam::WhiteNoiseFactor::WhiteNoiseFactor
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
Construct from measurement.
Definition: WhiteNoiseFactor.h:94
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:59
gtsam::WhiteNoiseFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: WhiteNoiseFactor.h:122
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::WhiteNoiseFactor::print
void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: WhiteNoiseFactor.h:111
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
gtsam::KeyFormatter
boost::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::WhiteNoiseFactor
Binary factor to estimate parameters of zero-mean Gaussian white noise.
Definition: WhiteNoiseFactor.h:39
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::WhiteNoiseFactor::error
double error(const Values &x) const override
Calculate the error of the factor, typically equal to log-likelihood.
Definition: WhiteNoiseFactor.h:127
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::logSqrt2PI
const double logSqrt2PI
constant needed below
Definition: WhiteNoiseFactor.h:26