Loading...
Searching...
No Matches
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;
using GaussianFactor::error;
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;
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>
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
Scatter.h
Maps global variable indices to slot indices.
HessianFactor-inl.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::EliminateCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:525
gtsam::EliminatePreferCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:548
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
gtsam::Ordering
Definition Ordering.h:34
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
gtsam::GaussianFactor::GaussianFactor
GaussianFactor()
Default constructor creates empty factor.
Definition GaussianFactor.h:46
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form).
Definition HessianFactor.h:101
gtsam::HessianFactor::This
HessianFactor This
Typedef to this class.
Definition HessianFactor.h:109
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Return the augmented information matrix represented by this GaussianFactor.
Definition HessianFactor.cpp:282
gtsam::HessianFactor::updateHessian
void updateHessian(HessianFactor *other) const
Update another Hessian factor.
Definition HessianFactor.h:328
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition HessianFactor.h:110
gtsam::HessianFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone this HessianFactor.
Definition HessianFactor.h:189
gtsam::HessianFactor::info
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
Definition HessianFactor.h:265
gtsam::HessianFactor::linearTerm
SymmetricBlockMatrix::Block linearTerm()
Return the complete linear term as described above.
Definition HessianFactor.h:259
gtsam::HessianFactor::constantTerm
double constantTerm() const
Return the constant term as described above.
Definition HessianFactor.h:230
gtsam::HessianFactor::linearTerm
SymmetricBlockMatrix::constBlock linearTerm() const
Return the complete linear term as described above.
Definition HessianFactor.h:251
gtsam::HessianFactor::~HessianFactor
~HessianFactor() override
Destructor.
Definition HessianFactor.h:186
gtsam::HessianFactor::HessianFactor
HessianFactor(const GaussianFactorGraph &factors)
Combine a set of factors into a single dense HessianFactor.
Definition HessianFactor.h:182
gtsam::HessianFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition HessianFactor.h:213
gtsam::HessianFactor::linearTerm
SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const
Return the part of linear term as described above corresponding to the requested variable.
Definition HessianFactor.h:244
gtsam::HessianFactor::info_
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
Definition HessianFactor.h:104
gtsam::HessianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition HessianFactor.h:108
gtsam::HessianFactor::updateHessian
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition HessianFactor.cpp:354
gtsam::HessianFactor::rows
size_t rows() const
Return the number of columns and rows of the Hessian matrix, including the information vector.
Definition HessianFactor.h:218
gtsam::HessianFactor::HessianFactor
HessianFactor()
default constructor for I/O
Definition HessianFactor.cpp:77
gtsam::HessianFactor::constBlock
SymmetricBlockMatrix::constBlock constBlock
A block from the Hessian matrix (const version).
Definition HessianFactor.h:112
gtsam::HessianFactor::info
SymmetricBlockMatrix & info()
Return non-const information matrix.
Definition HessianFactor.h:269
gtsam::HessianFactor::Block
SymmetricBlockMatrix::Block Block
A block from the Hessian matrix.
Definition HessianFactor.h:111
gtsam::HessianFactor::constantTerm
double & constantTerm()
Return the constant term as described above.
Definition HessianFactor.h:238
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55