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

#include <boost/make_shared.hpp>

namespace gtsam {

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

  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 KeyVector& 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,
      const Scatter& scatter);

    explicit HessianFactor(const GaussianFactorGraph& factors)
        : HessianFactor(factors, Scatter(factors)) {}

    ~HessianFactor() override {}

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

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

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

    double error(const VectorValues& c) const override;

    DenseIndex getDim(const_iterator variable) const override {
      return info_.getDim(std::distance(begin(), variable));
    }

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

    GaussianFactor::shared_ptr negate() const override;

    bool empty() const override { return size() == 0 /*|| rows() == 0*/; }

    double constantTerm() const {
      const auto view = info_.diagonalBlock(size());
      return view(0, 0);
    }

    double& constantTerm() { return info_.diagonalBlock(size())(0, 0); }

    SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const {
      assert(!empty());
      return info_.aboveDiagonalBlock(j - begin(), size());
    }

    SymmetricBlockMatrix::constBlock linearTerm() const {
      assert(!empty());
      // get the last column (except the bottom right block)
      return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
    }

    SymmetricBlockMatrix::Block linearTerm() {
      assert(!empty());
      return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
    }

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

    SymmetricBlockMatrix& info() { return info_; }

    Matrix augmentedInformation() const override;

    Eigen::SelfAdjointView<SymmetricBlockMatrix::constBlock, Eigen::Upper> informationView() const;

    Matrix information() const override;

    void hessianDiagonalAdd(VectorValues& d) const override;

    using Base::hessianDiagonal;

    void hessianDiagonal(double* d) const override;

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

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

    Matrix augmentedJacobian() const override;

    void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;

    void updateHessian(HessianFactor* other) const {
      assert(other);
      updateHessian(other->keys_, &other->info_);
    }

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

    VectorValues gradientAtZero() const override;

    void gradientAtZero(double* d) const override;

    Vector gradient(Key key, const VectorValues& x) const override;

    boost::shared_ptr<GaussianConditional> eliminateCholesky(const Ordering& keys);

    VectorValues solve();

 private:
    void Allocate(const Scatter& scatter);

    HessianFactor(const Scatter& scatter);

    friend class NonlinearFactorGraph;
    friend class NonlinearClusterTree;

    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_);
    }
  };

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

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

template<>
struct traits<HessianFactor> : public Testable<HessianFactor> {};

} // \ namespace gtsam


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