HessianFactor.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/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/GaussianFactor.h>

#include <boost/make_shared.hpp>

namespace gtsam {

  // Forward declarations
  class Ordering;
  class JacobianFactor;
  class HessianFactor;
  class GaussianConditional;
  class GaussianBayesNet;
  class GaussianFactorGraph;

  GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
    EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys);

  GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
    EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);

  struct GTSAM_EXPORT SlotEntry {
    size_t slot, dimension;
    SlotEntry(size_t _slot, size_t _dimension)
    : slot(_slot), dimension(_dimension) {}
    std::string toString() const;
  };

  class Scatter: public FastMap<Key, SlotEntry> {
  public:
    Scatter() {
    }
    Scatter(const GaussianFactorGraph& gfg,
        boost::optional<const Ordering&> ordering = boost::none);
  };

  class GTSAM_EXPORT HessianFactor : public GaussianFactor {
  protected:

    SymmetricBlockMatrix info_; 

  public:

    typedef GaussianFactor Base; 
    typedef HessianFactor This; 
    typedef boost::shared_ptr<This> shared_ptr; 
    typedef SymmetricBlockMatrix::Block Block; 
    typedef SymmetricBlockMatrix::constBlock constBlock; 


    HessianFactor();

    HessianFactor(Key j, const Matrix& G, const Vector& g, double f);

    HessianFactor(Key j, const Vector& mu, const Matrix& Sigma);

    HessianFactor(Key j1, Key j2,
        const Matrix& G11, const Matrix& G12, const Vector& g1,
        const Matrix& G22, const Vector& g2, double f);

    HessianFactor(Key j1, Key j2, Key j3,
        const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
        const Matrix& G22, const Matrix& G23, const Vector& g2,
        const Matrix& G33, const Vector& g3, double f);

    HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
        const std::vector<Vector>& gs, double f);

    template<typename KEYS>
    HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation);

    explicit HessianFactor(const JacobianFactor& cg);

    explicit HessianFactor(const GaussianFactor& factor);

    explicit HessianFactor(const GaussianFactorGraph& factors,
      boost::optional<const Scatter&> scatter = boost::none);

    virtual ~HessianFactor() {}

    virtual GaussianFactor::shared_ptr clone() const {
      return boost::make_shared<HessianFactor>(*this); }

    virtual void print(const std::string& s = "",
        const KeyFormatter& formatter = DefaultKeyFormatter) const;

    virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;

    virtual double error(const VectorValues& c) const; 
    virtual DenseIndex getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); }

    size_t rows() const { return info_.rows(); }

    virtual GaussianFactor::shared_ptr negate() const;
    
    virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; }

    constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }

    Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); }

    SymmetricBlockMatrix::constBlock info() const { return info_.full(); }

    SymmetricBlockMatrix::Block info() { return info_.full(); }

    double constantTerm() const { return info_(this->size(), this->size())(0,0); }

    double& constantTerm() { return info_(this->size(), this->size())(0,0); }

    constBlock::OffDiagonal::ColXpr linearTerm(const_iterator j) const {
      return info_(j-begin(), size()).knownOffDiagonal().col(0); }

    Block::OffDiagonal::ColXpr linearTerm(iterator j) {
      return info_(j-begin(), size()).knownOffDiagonal().col(0); }

    constBlock::OffDiagonal::ColXpr linearTerm() const {
      return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); }

    Block::OffDiagonal::ColXpr linearTerm() {
      return info_.range(0, this->size(), this->size(), this->size() + 1).knownOffDiagonal().col(0); }
    
    virtual Matrix augmentedInformation() const;

    virtual Matrix information() const;

    virtual VectorValues hessianDiagonal() const;

    /* ************************************************************************* */
    virtual void hessianDiagonal(double* d) const;

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

    virtual std::pair<Matrix, Vector> jacobian() const;

    virtual Matrix augmentedJacobian() const;

    const SymmetricBlockMatrix& matrixObject() const { return info_; }

    void updateATA(const JacobianFactor& update, const Scatter& scatter);

    void updateATA(const HessianFactor& update, const Scatter& scatter);

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

    void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const;

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

    VectorValues gradientAtZero() const;

    virtual void gradientAtZero(double* d) const;

    friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
      EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);

    friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
      EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys);

  private:

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

}

#include <gtsam/linear/HessianFactor-inl.h>