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/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h>  // Included here instead of fw-declared so we can use Errors::iterator
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
 
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 KeySet Keys;
    Keys keys() const;
 
    /* return a map of (Key, dimension) */
    std::map<Key, size_t> getKeyDimMap() const;
 
    double error(const VectorValues& x) const {
      double total_error = 0.;
      for(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<std::tuple<int, int, double> > sparseJacobian(
        const Ordering& ordering, size_t& nrows, size_t& ncols) const;
 
    std::vector<std::tuple<int, int, double> > sparseJacobian() const;
 
    Matrix sparseJacobian_() const;
 
    Matrix augmentedJacobian(const Ordering& ordering) const;
    
    Matrix augmentedJacobian() const;
 
    std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
 
    std::pair<Matrix,Vector> jacobian() const;
 
    Matrix augmentedHessian(const Ordering& ordering) const;
 
    Matrix augmentedHessian() const;
 
    std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
 
    std::pair<Matrix,Vector> hessian() const;
 
    virtual VectorValues hessianDiagonal() const;
 
    virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
 
    VectorValues optimize(
      const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
 
    VectorValues optimize(const Ordering&,
      const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
 
    VectorValues optimizeDensely() 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 multiplyInPlace(const VectorValues& x, Errors& e) const;
 
    void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
 
    void printErrors(
        const VectorValues& x,
        const std::string& str = "GaussianFactorGraph: ",
        const KeyFormatter& keyFormatter = DefaultKeyFormatter,
        const std::function<bool(const Factor* /*factor*/,
                                 double /*whitenedError*/, size_t /*index*/)>&
            printCondition =
                [](const Factor*, double, size_t) { return true; }) 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);
    }
 
  public:
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
   VectorValues optimize(boost::none_t,
                         const Eliminate& function =
                             EliminationTraitsType::DefaultEliminate) const {
     return optimize(function);
   }
#endif
 
  };
 
  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);
 
template<>
struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
};
 
} // \ namespace gtsam