gtsam 4.1.1 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
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
19#pragma once
20
23#include <vector>
24
25namespace gtsam {
26
27template<size_t D>
29
30public:
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 const Scatter& scatter)
81 : HessianFactor(factors, scatter) {
82 checkInvariants();
83 }
84
87 : HessianFactor(factors) {
88 checkInvariants();
89 }
90
91private:
92
94 void checkInvariants() {
95 if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
96 throw std::invalid_argument(
97 "RegularHessianFactor constructor was given non-regular factors");
98 }
99
100 // Use Eigen magic to access raw memory
101 typedef Eigen::Map<VectorD> DMap;
102 typedef Eigen::Map<const VectorD> ConstDMap;
103
104 // Scratch space for multiplyHessianAdd
107 mutable std::vector<VectorD> y_;
108
109public:
110
112 void multiplyHessianAdd(double alpha, const VectorValues& x,
113 VectorValues& y) const override {
115 }
116
118 void multiplyHessianAdd(double alpha, const double* x,
119 double* yvalues) const {
120 // Create a vector of temporary y_ values, corresponding to rows i
121 y_.resize(size());
122 for(VectorD & yi: y_)
123 yi.setZero();
124
125 // Accessing the VectorValues one by one is expensive
126 // So we will loop over columns to access x only once per column
127 // And fill the above temporary y_ values, to be added into yvalues after
128 VectorD xj(D);
129 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
130 Key key = keys_[j];
131 const double* xj = x + key * D;
132 DenseIndex i = 0;
133 for (; i < j; ++i)
134 y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj);
135 // blocks on the diagonal are only half
136 y_[i] += info_.diagonalBlock(j) * ConstDMap(xj);
137 // for below diagonal, we take transpose block from upper triangular part
138 for (i = j + 1; i < (DenseIndex) size(); ++i)
139 y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj);
140 }
141
142 // copy to yvalues
143 for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
144 Key key = keys_[i];
145 DMap(yvalues + key * D) += alpha * y_[i];
146 }
147 }
148
150 void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
151 std::vector<size_t> offsets) const {
152
153 // Create a vector of temporary y_ values, corresponding to rows i
154 y_.resize(size());
155 for(VectorD & yi: y_)
156 yi.setZero();
157
158 // Accessing the VectorValues one by one is expensive
159 // So we will loop over columns to access x only once per column
160 // And fill the above temporary y_ values, to be added into yvalues after
161 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
162 DenseIndex i = 0;
163 for (; i < j; ++i)
164 y_[i] += info_.aboveDiagonalBlock(i, j)
165 * ConstDMap(x + offsets[keys_[j]],
166 offsets[keys_[j] + 1] - offsets[keys_[j]]);
167 // blocks on the diagonal are only half
168 y_[i] += info_.diagonalBlock(j)
169 * ConstDMap(x + offsets[keys_[j]],
170 offsets[keys_[j] + 1] - offsets[keys_[j]]);
171 // for below diagonal, we take transpose block from upper triangular part
172 for (i = j + 1; i < (DenseIndex) size(); ++i)
173 y_[i] += info_.aboveDiagonalBlock(j, i).transpose()
174 * ConstDMap(x + offsets[keys_[j]],
175 offsets[keys_[j] + 1] - offsets[keys_[j]]);
176 }
177
178 // copy to yvalues
179 for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
180 DMap(yvalues + offsets[keys_[i]],
181 offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
182 }
183
185 void hessianDiagonal(double* d) const override {
186
187 // Loop over all variables in the factor
188 for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
189 Key j = keys_[pos];
190 // Get the diagonal block, and insert its diagonal
191 DMap(d + D * j) += info_.diagonal(pos);
192 }
193 }
194
196 void gradientAtZero(double* d) const override {
197
198 // Loop over all variables in the factor
199 for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
200 Key j = keys_[pos];
201 // Get the diagonal block, and insert its diagonal
202 DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());;
203 }
204 }
205
206 /* ************************************************************************* */
207
208};
209// end class RegularHessianFactor
210
211// traits
212template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
213 RegularHessianFactor<D> > {
214};
215
216}
217
Contains the HessianFactor class, a general quadratic factor.
JacobianFactor class with fixed sized blcoks.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:75
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Definition: SymmetricBlockMatrix.h:52
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition: SymmetricBlockMatrix.h:150
DenseIndex cols() const
Column size.
Definition: SymmetricBlockMatrix.h:122
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:155
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:140
DenseIndex nBlocks() const
Block count.
Definition: SymmetricBlockMatrix.h:125
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:125
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:73
size_t size() const
Definition: Factor.h:136
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
Matrix augmentedInformation() const override
Return the augmented information matrix represented by this GaussianFactor.
Definition: HessianFactor.cpp:281
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
Definition: HessianFactor.h:104
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition: HessianFactor.cpp:390
Definition: RegularHessianFactor.h:28
RegularHessianFactor(const GaussianFactorGraph &factors)
Construct from a GaussianFactorGraph.
Definition: RegularHessianFactor.h:86
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition: RegularHessianFactor.h:112
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
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:150
void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
y += alpha * A'*A*x
Definition: RegularHessianFactor.h:118
void hessianDiagonal(double *d) const override
Return the diagonal of the Hessian for this factor (raw memory version)
Definition: RegularHessianFactor.h:185
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
Definition: RegularHessianFactor.h:196
RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
Construct from a GaussianFactorGraph.
Definition: RegularHessianFactor.h:79
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
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
RegularHessianFactor(const RegularJacobianFactor< D > &jf)
Construct from RegularJacobianFactor.
Definition: RegularHessianFactor.h:75
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition: RegularJacobianFactor.h:32
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition: Scatter.h:49
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74