gtsam  4.1.0
gtsam
HessianFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
22 #include <gtsam/linear/Scatter.h>
24 #include <gtsam/base/FastVector.h>
25 
26 #include <boost/make_shared.hpp>
27 
28 namespace gtsam {
29 
30  // Forward declarations
31  class Ordering;
32  class JacobianFactor;
33  class HessianFactor;
34  class GaussianConditional;
35  class GaussianBayesNet;
36  class GaussianFactorGraph;
37 
101  class GTSAM_EXPORT HessianFactor : public GaussianFactor {
102  protected:
103 
105 
106  public:
107 
109  typedef HessianFactor This;
110  typedef boost::shared_ptr<This> shared_ptr;
111  typedef SymmetricBlockMatrix::Block Block;
112  typedef SymmetricBlockMatrix::constBlock constBlock;
113 
114 
116  HessianFactor();
117 
123  HessianFactor(Key j, const Matrix& G, const Vector& g, double f);
124 
128  HessianFactor(Key j, const Vector& mu, const Matrix& Sigma);
129 
145  HessianFactor(Key j1, Key j2,
146  const Matrix& G11, const Matrix& G12, const Vector& g1,
147  const Matrix& G22, const Vector& g2, double f);
148 
153  HessianFactor(Key j1, Key j2, Key j3,
154  const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1,
155  const Matrix& G22, const Matrix& G23, const Vector& g2,
156  const Matrix& G33, const Vector& g3, double f);
157 
162  HessianFactor(const KeyVector& js, const std::vector<Matrix>& Gs,
163  const std::vector<Vector>& gs, double f);
164 
167  template<typename KEYS>
168  HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation);
169 
171  explicit HessianFactor(const JacobianFactor& cg);
172 
175  explicit HessianFactor(const GaussianFactor& factor);
176 
178  explicit HessianFactor(const GaussianFactorGraph& factors,
179  const Scatter& scatter);
180 
182  explicit HessianFactor(const GaussianFactorGraph& factors)
183  : HessianFactor(factors, Scatter(factors)) {}
184 
186  virtual ~HessianFactor() {}
187 
190  return boost::make_shared<HessianFactor>(*this); }
191 
193  void print(const std::string& s = "",
194  const KeyFormatter& formatter = DefaultKeyFormatter) const override;
195 
197  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
198 
203  double error(const VectorValues& c) const override;
204 
210  DenseIndex getDim(const_iterator variable) const override {
211  return info_.getDim(std::distance(begin(), variable));
212  }
213 
215  size_t rows() const { return info_.rows(); }
216 
222  GaussianFactor::shared_ptr negate() const override;
223 
225  bool empty() const override { return size() == 0 /*|| rows() == 0*/; }
226 
230  double constantTerm() const {
231  const auto view = info_.diagonalBlock(size());
232  return view(0, 0);
233  }
234 
238  double& constantTerm() { return info_.diagonalBlock(size())(0, 0); }
239 
244  SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const {
245  assert(!empty());
246  return info_.aboveDiagonalBlock(j - begin(), size());
247  }
248 
251  SymmetricBlockMatrix::constBlock linearTerm() const {
252  assert(!empty());
253  // get the last column (except the bottom right block)
254  return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
255  }
256 
259  SymmetricBlockMatrix::Block linearTerm() {
260  assert(!empty());
261  return info_.aboveDiagonalRange(0, size(), size(), size() + 1);
262  }
263 
265  const SymmetricBlockMatrix& info() const { return info_; }
266 
269  SymmetricBlockMatrix& info() { return info_; }
270 
286  Matrix augmentedInformation() const override;
287 
289  Eigen::SelfAdjointView<SymmetricBlockMatrix::constBlock, Eigen::Upper> informationView() const;
290 
294  Matrix information() const override;
295 
297  void hessianDiagonalAdd(VectorValues& d) const override;
298 
300  using Base::hessianDiagonal;
301 
303  void hessianDiagonal(double* d) const override;
304 
306  std::map<Key,Matrix> hessianBlockDiagonal() const override;
307 
309  std::pair<Matrix, Vector> jacobian() const override;
310 
316  Matrix augmentedJacobian() const override;
317 
323  void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
324 
328  void updateHessian(HessianFactor* other) const {
329  assert(other);
330  updateHessian(other->keys_, &other->info_);
331  }
332 
334  void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const override;
335 
337  VectorValues gradientAtZero() const override;
338 
340  void gradientAtZero(double* d) const override;
341 
346  Vector gradient(Key key, const VectorValues& x) const override;
347 
352  boost::shared_ptr<GaussianConditional> eliminateCholesky(const Ordering& keys);
353 
355  VectorValues solve();
356 
357  private:
359  void Allocate(const Scatter& scatter);
360 
362  HessianFactor(const Scatter& scatter);
363 
364  friend class NonlinearFactorGraph;
365  friend class NonlinearClusterTree;
366 
368  friend class boost::serialization::access;
369  template<class ARCHIVE>
370  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
371  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor);
372  ar & BOOST_SERIALIZATION_NVP(info_);
373  }
374  };
375 
392 GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
393  EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
394 
410 GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> >
411  EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys);
412 
414 template<>
415 struct traits<HessianFactor> : public Testable<HessianFactor> {};
416 
417 } // \ namespace gtsam
418 
419 
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
Scatter.h
Maps global variable indices to slot indices.
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Ordering
Definition: Ordering.h:34
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:110
gtsam::HessianFactor::constBlock
SymmetricBlockMatrix::constBlock constBlock
A block from the Hessian matrix (const version)
Definition: HessianFactor.h:112
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
constBlock aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
Get a range [i,j) from the matrix. Indices are in block units.
Definition: SymmetricBlockMatrix.h:175
gtsam::HessianFactor::linearTerm
SymmetricBlockMatrix::constBlock linearTerm() const
Return the complete linear term as described above.
Definition: HessianFactor.h:251
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::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::HessianFactor::linearTerm
SymmetricBlockMatrix::Block linearTerm()
Return the complete linear term as described above.
Definition: HessianFactor.h:259
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
gtsam::HessianFactor::HessianFactor
HessianFactor(const GaussianFactorGraph &factors)
Combine a set of factors into a single dense HessianFactor.
Definition: HessianFactor.h:182
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::HessianFactor::empty
bool empty() const override
Check if the factor is empty.
Definition: HessianFactor.h:225
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::SymmetricBlockMatrix::getDim
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition: SymmetricBlockMatrix.h:128
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:210
gtsam::HessianFactor::constantTerm
double constantTerm() const
Return the constant term as described above.
Definition: HessianFactor.h:230
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::HessianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition: HessianFactor.h:108
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::HessianFactor::constantTerm
double & constantTerm()
Return the constant term as described above.
Definition: HessianFactor.h:238
gtsam::HessianFactor::This
HessianFactor This
Typedef to this class.
Definition: HessianFactor.h:109
gtsam::HessianFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone this HessianFactor.
Definition: HessianFactor.h:189
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:155
HessianFactor-inl.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition: Scatter.h:49
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
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: SymmetricBlockMatrix.h:119
gtsam::KeyFormatter
boost::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::HessianFactor::Block
SymmetricBlockMatrix::Block Block
A block from the Hessian matrix.
Definition: HessianFactor.h:111
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:140
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::HessianFactor::info
SymmetricBlockMatrix & info()
Return non-const information matrix.
Definition: HessianFactor.h:269
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
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:215
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
gtsam::HessianFactor::~HessianFactor
virtual ~HessianFactor()
Destructor.
Definition: HessianFactor.h:186
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
gtsam::NonlinearClusterTree
Definition: NonlinearClusterTree.h:14
gtsam::HessianFactor::updateHessian
void updateHessian(HessianFactor *other) const
Update another Hessian factor.
Definition: HessianFactor.h:328
gtsam::HessianFactor::info
const SymmetricBlockMatrix & info() const
Return underlying information matrix.
Definition: HessianFactor.h:265