WhiteNoiseFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <cmath>
 
namespace gtsam {
 
  const double logSqrt2PI = log(std::sqrt(2.0 * M_PI)); 
 
  class WhiteNoiseFactor: public NonlinearFactor {
 
  private:
 
    double z_; 
 
    Key meanKey_; 
    Key precisionKey_; 
 
    typedef NonlinearFactor Base;
 
  public:
 
    static double f(double z, double u, double p) {
      return logSqrt2PI - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p;
    }
 
    static HessianFactor::shared_ptr linearize(double z, double u, double p,
        Key j1, Key j2) {
      double e = u - z, e2 = e * e;
      double c = 2 * logSqrt2PI - log(p) + e2 * p;
      Vector g1 = (Vector(1) << -e * p).finished();
      Vector g2 = (Vector(1) <<  0.5 / p - 0.5 * e2).finished();
      Matrix G11 = (Matrix(1, 1) << p).finished();
      Matrix G12 = (Matrix(1, 1) << e).finished();
      Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished();
      return HessianFactor::shared_ptr(
          new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
    }
 
 
    WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
        Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
    }
 
 
    ~WhiteNoiseFactor() override {
    }
 
 
    void print(const std::string& p = "WhiteNoiseFactor",
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
      Base::print(p, keyFormatter);
      std::cout << p + ".z: " << z_ << std::endl;
    }
 
 
    size_t dim() const override {
      return 2;
    }
 
    double error(const Values& x) const override {
      return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
    }
 
    virtual Vector unwhitenedError(const Values& x) const {
      return (Vector(1) << std::sqrt(2 * error(x))).finished();
    }
 
//    virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
//      const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
//      return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
//    }
 
 
    std::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
      double u = x.at<double>(meanKey_);
      double p = x.at<double>(precisionKey_);
      Key j1 = meanKey_;
      Key j2 = precisionKey_;
      return linearize(z_, u, p, j1, j2);
    }
 
    // TODO: Frank commented this out for now, can it go?
    //    /// @return a deep copy of this factor
    //    gtsam::NonlinearFactor::shared_ptr clone() const override {
    //      return std::static_pointer_cast<gtsam::NonlinearFactor>(
    //          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
 
  };
// WhiteNoiseFactor
 
}// namespace gtsam