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>