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

 * -------------------------------------------------------------------------- */

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/LieScalar.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);
      Vector g2 = (Vector(1) <<  0.5 / p - 0.5 * e2);
      Matrix G11 = (Matrix(1, 1) << p);
      Matrix G12 = (Matrix(1, 1) << e);
      Matrix G22 = (Matrix(1, 1) << 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) {
    }


    virtual ~WhiteNoiseFactor() {
    }


    void print(const std::string& p = "WhiteNoiseFactor",
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
      Base::print(p, keyFormatter);
      std::cout << p + ".z: " << z_ << std::endl;
    }


    virtual size_t dim() const {
      return 2;
    }

    inline double error(const Values& x) const {
      return f(z_, x.at<LieScalar>(meanKey_), x.at<LieScalar>(precisionKey_));
    }

    virtual Vector unwhitenedError(const Values& x) const {
      return (Vector(1) << 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));
//    }


    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
      double u = x.at<LieScalar>(meanKey_);
      double p = x.at<LieScalar>(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
    //    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
    //      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
    //          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }


  };
// WhiteNoiseFactor

}// namespace gtsam