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 
47 
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  ~WhiteNoiseFactor() override {
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 
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...
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
#define M_PI
Definition: main.h:78
EIGEN_DEVICE_FUNC const LogReturnType log() const
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
virtual Vector unwhitenedError(const Values &x) const
const double logSqrt2PI
constant needed below
Eigen::VectorXd Vector
Definition: Vector.h:38
const ValueType at(Key j) const
Definition: Values-inl.h:342
~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.
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
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
Contains the HessianFactor class, a general quadratic factor.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize returns a Hessianfactor that is an approximation of error(p)
traits
Definition: chartTesting.h:28
Non-linear factor base classes.
Key precisionKey_
key by which to access precision variable
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.
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:61


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:51:25