GaussianFactorGraph.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/inference/FactorGraph.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator

namespace gtsam {

  // Forward declarations
  class GaussianFactorGraph;
  class GaussianFactor;
  class GaussianConditional;
  class GaussianBayesNet;
  class GaussianEliminationTree;
  class GaussianBayesTree;
  class GaussianJunctionTree;

  /* ************************************************************************* */
  template<> struct EliminationTraits<GaussianFactorGraph>
  {
    typedef GaussianFactor FactorType;                   
    typedef GaussianFactorGraph FactorGraphType;         
    typedef GaussianConditional ConditionalType;         
    typedef GaussianBayesNet BayesNetType;               
    typedef GaussianEliminationTree EliminationTreeType; 
    typedef GaussianBayesTree BayesTreeType;             
    typedef GaussianJunctionTree JunctionTreeType;       
    static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
      DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
        return EliminatePreferCholesky(factors, keys); }
  };

  /* ************************************************************************* */
  class GTSAM_EXPORT GaussianFactorGraph :
    public FactorGraph<GaussianFactor>,
    public EliminateableFactorGraph<GaussianFactorGraph>
  {
  public:

    typedef GaussianFactorGraph This; 
    typedef FactorGraph<GaussianFactor> Base; 
    typedef EliminateableFactorGraph<This> BaseEliminateable; 
    typedef boost::shared_ptr<This> shared_ptr; 

    GaussianFactorGraph() {}

    template<typename ITERATOR>
    GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}

    template<class CONTAINER>
    explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}

    template<class DERIVEDFACTOR>
    GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}

    virtual ~GaussianFactorGraph() {}


    bool equals(const This& fg, double tol = 1e-9) const;


    void add(const GaussianFactor& factor) { push_back(factor.clone()); }

    void add(const sharedFactor& factor) { push_back(factor); }

    void add(const Vector& b) {
      add(JacobianFactor(b)); }

    void add(Key key1, const Matrix& A1,
        const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
      add(JacobianFactor(key1,A1,b,model)); }

    void add(Key key1, const Matrix& A1,
        Key key2, const Matrix& A2,
        const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
      add(JacobianFactor(key1,A1,key2,A2,b,model)); }

    void add(Key key1, const Matrix& A1,
        Key key2, const Matrix& A2,
        Key key3, const Matrix& A3,
        const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
      add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); }

    template<class TERMS>
    void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
      add(JacobianFactor(terms,b,model)); }

    typedef FastSet<Key> Keys;
    Keys keys() const;

    /* return a map of (Key, dimension) */
    std::map<Key, size_t> getKeyDimMap() const;

    std::vector<size_t> getkeydim() const;

    double error(const VectorValues& x) const {
      double total_error = 0.;
      BOOST_FOREACH(const sharedFactor& factor, *this){
        if(factor)
          total_error += factor->error(x);
      }
      return total_error;
    }

    double probPrime(const VectorValues& c) const {
      return exp(-0.5 * error(c));
    }

    virtual GaussianFactorGraph clone() const;

    virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;

    GaussianFactorGraph negate() const;


    std::vector<boost::tuple<size_t, size_t, double> > sparseJacobian() const;

    Matrix sparseJacobian_() const;

    Matrix augmentedJacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;

    std::pair<Matrix,Vector> jacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;

    Matrix augmentedHessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;

    std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;

    virtual VectorValues hessianDiagonal() const;

    virtual std::map<Key,Matrix> hessianBlockDiagonal() const;

    VectorValues optimize(OptionalOrdering ordering = boost::none,
      const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;

    VectorValues gradient(const VectorValues& x0) const;

    virtual VectorValues gradientAtZero() const;

    VectorValues optimizeGradientSearch() const;

    VectorValues transposeMultiply(const Errors& e) const;

    void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;

    Errors gaussianErrors(const VectorValues& x) const;

    Errors operator*(const VectorValues& x) const;

    void multiplyHessianAdd(double alpha, const VectorValues& x,
        VectorValues& y) const;

    void multiplyHessianAdd(double alpha, const double* x,
        double* y) const;

    void multiplyInPlace(const VectorValues& x, Errors& e) const;

    void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;


  private:
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int version) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    }

  };

  GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);

  /****** Linear Algebra Opeations ******/

  //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
  //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);

} // namespace gtsam