gtsam  4.0.0
gtsam
RegularHessianFactor.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 
23 #include <vector>
24 
25 namespace gtsam {
26 
27 template<size_t D>
29 
30 public:
31 
32  typedef Eigen::Matrix<double, D, 1> VectorD;
33  typedef Eigen::Matrix<double, D, D> MatrixD;
34 
40  const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
41  HessianFactor(js, Gs, gs, f) {
42  checkInvariants();
43  }
44 
49  RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
50  const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
51  HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
52  }
53 
59  const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
60  const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
61  const MatrixD& G33, const VectorD& g3, double f) :
62  HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
63  }
64 
67  template<typename KEYS>
71  checkInvariants();
72  }
73 
76  : HessianFactor(jf) {}
77 
80  boost::optional<const Scatter&> scatter = boost::none)
81  : HessianFactor(factors, scatter) {
82  checkInvariants();
83  }
84 
85 private:
86 
88  void checkInvariants() {
89  if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
90  throw std::invalid_argument(
91  "RegularHessianFactor constructor was given non-regular factors");
92  }
93 
94  // Use Eigen magic to access raw memory
95  typedef Eigen::Map<VectorD> DMap;
96  typedef Eigen::Map<const VectorD> ConstDMap;
97 
98  // Scratch space for multiplyHessianAdd
99  // According to link below this is thread-safe.
100  // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
101  mutable std::vector<VectorD> y_;
102 
103 public:
104 
106  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
107  VectorValues& y) const {
109  }
110 
112  void multiplyHessianAdd(double alpha, const double* x,
113  double* yvalues) const {
114  // Create a vector of temporary y_ values, corresponding to rows i
115  y_.resize(size());
116  for(VectorD & yi: y_)
117  yi.setZero();
118 
119  // Accessing the VectorValues one by one is expensive
120  // So we will loop over columns to access x only once per column
121  // And fill the above temporary y_ values, to be added into yvalues after
122  VectorD xj(D);
123  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
124  Key key = keys_[j];
125  const double* xj = x + key * D;
126  DenseIndex i = 0;
127  for (; i < j; ++i)
128  y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj);
129  // blocks on the diagonal are only half
130  y_[i] += info_.diagonalBlock(j) * ConstDMap(xj);
131  // for below diagonal, we take transpose block from upper triangular part
132  for (i = j + 1; i < (DenseIndex) size(); ++i)
133  y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj);
134  }
135 
136  // copy to yvalues
137  for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
138  Key key = keys_[i];
139  DMap(yvalues + key * D) += alpha * y_[i];
140  }
141  }
142 
144  void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
145  std::vector<size_t> offsets) const {
146 
147  // Create a vector of temporary y_ values, corresponding to rows i
148  y_.resize(size());
149  for(VectorD & yi: y_)
150  yi.setZero();
151 
152  // Accessing the VectorValues one by one is expensive
153  // So we will loop over columns to access x only once per column
154  // And fill the above temporary y_ values, to be added into yvalues after
155  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
156  DenseIndex i = 0;
157  for (; i < j; ++i)
158  y_[i] += info_.aboveDiagonalBlock(i, j)
159  * ConstDMap(x + offsets[keys_[j]],
160  offsets[keys_[j] + 1] - offsets[keys_[j]]);
161  // blocks on the diagonal are only half
162  y_[i] += info_.diagonalBlock(j)
163  * ConstDMap(x + offsets[keys_[j]],
164  offsets[keys_[j] + 1] - offsets[keys_[j]]);
165  // for below diagonal, we take transpose block from upper triangular part
166  for (i = j + 1; i < (DenseIndex) size(); ++i)
167  y_[i] += info_.aboveDiagonalBlock(j, i).transpose()
168  * ConstDMap(x + offsets[keys_[j]],
169  offsets[keys_[j] + 1] - offsets[keys_[j]]);
170  }
171 
172  // copy to yvalues
173  for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
174  DMap(yvalues + offsets[keys_[i]],
175  offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
176  }
177 
179  virtual void hessianDiagonal(double* d) const {
180 
181  // Loop over all variables in the factor
182  for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
183  Key j = keys_[pos];
184  // Get the diagonal block, and insert its diagonal
185  DMap(d + D * j) += info_.diagonal(pos);
186  }
187  }
188 
190  virtual void gradientAtZero(double* d) const {
191 
192  // Loop over all variables in the factor
193  for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
194  Key j = keys_[pos];
195  // Get the diagonal block, and insert its diagonal
196  DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());;
197  }
198  }
199 
200  /* ************************************************************************* */
201 
202 };
203 // end class RegularHessianFactor
204 
205 // traits
206 template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
207  RegularHessianFactor<D> > {
208 };
209 
210 }
211 
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
Definition: HessianFactor.h:104
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:155
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:63
JacobianFactor class with fixed sized blcoks.
DenseIndex cols() const
Column size.
Definition: SymmetricBlockMatrix.h:122
RegularHessianFactor(const RegularJacobianFactor< D > &jf)
Construct from RegularJacobianFactor.
Definition: RegularHessianFactor.h:75
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const
y += alpha * A'*A*x
Definition: HessianFactor.cpp:396
RegularHessianFactor(Key j1, Key j2, Key j3, const MatrixD &G11, const MatrixD &G12, const MatrixD &G13, const VectorD &g1, const MatrixD &G22, const MatrixD &G23, const VectorD &g2, const MatrixD &G33, const VectorD &g3, double f)
Construct a ternary factor.
Definition: RegularHessianFactor.h:58
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)
Construct a binary factor.
Definition: RegularHessianFactor.h:49
void multiplyHessianAdd(double alpha, const double *x, double *yvalues, std::vector< size_t > offsets) const
Raw memory version, with offsets TODO document reasoning.
Definition: RegularHessianFactor.h:144
RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
Construct an n-way factor.
Definition: RegularHessianFactor.h:39
Definition: RegularHessianFactor.h:28
size_t size() const
Definition: Factor.h:129
RegularHessianFactor(const GaussianFactorGraph &factors, boost::optional< const Scatter & > scatter=boost::none)
Construct from a GaussianFactorGraph.
Definition: RegularHessianFactor.h:79
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
virtual void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const
y += alpha * A'*A*x
Definition: RegularHessianFactor.h:106
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:56
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:140
Contains the HessianFactor class, a general quadratic factor.
void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
y += alpha * A'*A*x
Definition: RegularHessianFactor.h:112
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition: SymmetricBlockMatrix.h:150
Definition: SymmetricBlockMatrix.h:51
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
virtual void gradientAtZero(double *d) const
Add gradient at zero to d TODO: is it really the goal to add ??
Definition: RegularHessianFactor.h:190
RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
Constructor with an arbitrary number of keys and with the augmented information matrix specified as a...
Definition: RegularHessianFactor.h:68
virtual void hessianDiagonal(double *d) const
Return the diagonal of the Hessian for this factor (raw memory version)
Definition: RegularHessianFactor.h:179
DenseIndex nBlocks() const
Block count.
Definition: SymmetricBlockMatrix.h:125
virtual Matrix augmentedInformation() const
Return the augmented information matrix represented by this GaussianFactor.
Definition: HessianFactor.cpp:289
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition: RegularJacobianFactor.h:32
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101