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 <utility>
 
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 std::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);

    template <typename KEYS>
    HessianFactor(const KEYS& keys,
                  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 std::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;

    using GaussianFactor::error;

    double error(const VectorValues& c) const override;

    double deltaError(const VectorValues& c, double* oldError = nullptr,
                      double* newError = nullptr) 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;

    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 {
#ifndef NDEBUG
      if(empty()) throw;
#endif
      return info_.aboveDiagonalBlock(j - begin(), size());
    }

    SymmetricBlockMatrix::constBlock linearTerm() const {
#ifndef NDEBUG
      if(empty()) throw;
#endif
      // get the last column (except the bottom right block)
      return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
    }

    SymmetricBlockMatrix::Block linearTerm() {
#ifndef NDEBUG
      if(empty()) throw;
#endif
      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(const KeyVector& keys, SymmetricBlockMatrix* info,
                       DenseIndex beginCol, DenseIndex endCol) const override;

    void updateHessian(HessianFactor* other) const {
#ifndef NDEBUG
      if(!other) throw;
#endif
      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;

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

    VectorValues solve();
 
 private:
    static void CheckAugmentedInformation(
        size_t keyCount, const SymmetricBlockMatrix& augmentedInformation);

    void Allocate(const Scatter& scatter);

    HessianFactor(const Scatter& scatter);
 
    friend class NonlinearFactorGraph;
    friend class NonlinearClusterTree;
 
#if 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(GaussianFactor);
      ar & BOOST_SERIALIZATION_NVP(info_);
    }
#endif
  };

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

GTSAM_EXPORT std::pair<std::shared_ptr<GaussianConditional>, std::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>