61 static double f(
double z,
double u,
double p) {
62 return logSqrt2PI - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p;
77 double e = u - z, e2 = e * e;
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();
97 Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
113 void print(
const std::string& p =
"WhiteNoiseFactor",
114 const KeyFormatter& keyFormatter = DefaultKeyFormatter)
const override {
116 std::cout << p +
".z: " << z_ << std::endl;
124 size_t dim()
const override {
130 return f(z_, x.
at<
double>(meanKey_), x.
at<
double>(precisionKey_));
141 return (Vector(1) << std::sqrt(2 *
error(x))).finished();
159 double u = x.
at<
double>(meanKey_);
160 double p = x.
at<
double>(precisionKey_);
162 Key j2 = precisionKey_;
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
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