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 
gtsam::WhiteNoiseFactor::~WhiteNoiseFactor
~WhiteNoiseFactor() override
Destructor.
Definition: WhiteNoiseFactor.h:105
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::WhiteNoiseFactor::print
void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: WhiteNoiseFactor.h:113
x
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
Definition: gnuplot_common_settings.hh:12
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
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
gtsam::HessianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:109
gtsam::Factor
Definition: Factor.h:70
gtsam::WhiteNoiseFactor::z_
double z_
Measurement.
Definition: WhiteNoiseFactor.h:45
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::WhiteNoiseFactor::meanKey_
Key meanKey_
key by which to access mean variable
Definition: WhiteNoiseFactor.h:47
gtsam::WhiteNoiseFactor::Base
NonlinearFactor Base
Definition: WhiteNoiseFactor.h:50
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
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::logSqrt2PI
const double logSqrt2PI
constant needed below
Definition: WhiteNoiseFactor.h:28
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:35
gtsam::WhiteNoiseFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: WhiteNoiseFactor.h:124
NonlinearFactor.h
Non-linear factor base classes.
gtsam::WhiteNoiseFactor::linearize
std::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::precisionKey_
Key precisionKey_
key by which to access precision variable
Definition: WhiteNoiseFactor.h:48
gtsam::WhiteNoiseFactor
Binary factor to estimate parameters of zero-mean Gaussian white noise.
Definition: WhiteNoiseFactor.h:41
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
j1
double j1(double x)
Definition: j1.c:174
gtsam::WhiteNoiseFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x) const
Definition: WhiteNoiseFactor.h:140
gtsam::WhiteNoiseFactor::WhiteNoiseFactor
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
Definition: WhiteNoiseFactor.h:96
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
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::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
Author(s):
autogenerated on Fri Nov 1 2024 03:43:12