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 <cstddef>
#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<std::shared_ptr<ConditionalType>, std::shared_ptr<FactorType> >
      DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
        return EliminatePreferCholesky(factors, keys); }
    static Ordering DefaultOrderingFunc(
        const FactorGraphType& graph,
        std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
      return Ordering::Colamd((*variableIndex).get());
    }
  };

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

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

    
    GaussianFactorGraph() {}

    GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
    

    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) {}


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


    friend bool operator==(const GaussianFactorGraph& lhs,
                            const GaussianFactorGraph& rhs) {
      return lhs.isEqual(rhs);
    }

    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 probPrime(const VectorValues& c) const;

    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:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

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

  GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);

  /****** Linear Algebra Operations ******/

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