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{{-e * p}};
      Vector g2{{0.5 / p - 0.5 * e2}};
      Matrix G11{{p}};
      Matrix G12{{e}};
      Matrix G22{{0.5 / (p * p)}};
      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{{std::sqrt(2 * error(x))}};
    }

//    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