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 
26 namespace gtsam {
27 
28  const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
29 
42 
43  private:
44 
45  double z_;
46 
49 
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  std::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 std::static_pointer_cast<gtsam::NonlinearFactor>(
170  // gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
171 
173 
174  };
175 // WhiteNoiseFactor
176 
177 }// namespace gtsam
178 
void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
static double f(double z, double u, double p)
negative log likelihood as a function of mean and precision
static HessianFactor::shared_ptr linearize(double z, double u, double p, Key j1, Key j2)
linearize returns a Hessianfactor that approximates error Hessian is Taylor expansion is So f = 2 f...
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
const ValueType at(Key j) const
Definition: Values-inl.h:204
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
#define M_PI
Definition: main.h:106
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
EIGEN_DEVICE_FUNC const LogReturnType log() const
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
const double logSqrt2PI
constant needed below
virtual Vector unwhitenedError(const Values &x) const
Eigen::VectorXd Vector
Definition: Vector.h:38
~WhiteNoiseFactor() override
Destructor.
Key meanKey_
key by which to access mean variable
double error(const Values &x) const override
Calculate the error of the factor, typically equal to log-likelihood.
Contains the HessianFactor class, a general quadratic factor.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
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
traits
Definition: chartTesting.h:28
Non-linear factor base classes.
Key precisionKey_
key by which to access precision variable
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize returns a Hessianfactor that is an approximation of error(p)
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
float * p
A Gaussian factor using the canonical parameters (information form)
Binary factor to estimate parameters of zero-mean Gaussian white noise.
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
double z_
Measurement.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Pose3 g2(g1.expmap(h *V1_g1))
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:46