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 
34 
40  const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
41  HessianFactor(js, Gs, gs, f) {
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>
72  }
73 
76  : HessianFactor(jf) {}
77 
80  const Scatter& scatter)
81  : HessianFactor(factors, scatter) {
83  }
84 
89  }
90 
91 private:
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
103 
104  // Scratch space for multiplyHessianAdd
105  // According to link below this is thread-safe.
106  // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
107  mutable std::vector<VectorD> y_;
108 
109 public:
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)
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
212 template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
213  RegularHessianFactor<D> > {
214 };
215 
216 }
217 
gtsam::RegularHessianFactor::DMap
Eigen::Map< VectorD > DMap
Definition: RegularHessianFactor.h:101
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)
Definition: RegularHessianFactor.h:49
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Definition: HessianFactor.cpp:276
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
d
static const double d[K][N]
Definition: igam.h:11
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
gtsam::RegularHessianFactor
Definition: RegularHessianFactor.h:28
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::HessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: HessianFactor.cpp:385
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::RegularHessianFactor::y_
std::vector< VectorD > y_
Definition: RegularHessianFactor.h:107
gtsam::RegularHessianFactor::checkInvariants
void checkInvariants()
Check invariants after construction.
Definition: RegularHessianFactor.h:94
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:103
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const RegularJacobianFactor< D > &jf)
Construct from RegularJacobianFactor.
Definition: RegularHessianFactor.h:75
gtsam::VectorValues
Definition: VectorValues.h:74
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::RegularHessianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Definition: RegularHessianFactor.h:185
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::RegularHessianFactor::RegularHessianFactor
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)
Definition: RegularHessianFactor.h:58
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam::Scatter
Definition: Scatter.h:49
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition: SymmetricBlockMatrix.h:122
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
gtsam::RegularHessianFactor::ConstDMap
Eigen::Map< const VectorD > ConstDMap
Definition: RegularHessianFactor.h:102
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::RegularHessianFactor::VectorD
Eigen::Matrix< double, D, 1 > VectorD
Definition: RegularHessianFactor.h:32
gtsam::SymmetricBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition: SymmetricBlockMatrix.h:119
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
Definition: RegularHessianFactor.h:118
gtsam
traits
Definition: SFMdata.h:40
gtsam::RegularHessianFactor::multiplyHessianAdd
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
gtsam::Testable
Definition: Testable.h:152
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::traits
Definition: Group.h:36
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
gtsam::RegularHessianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Add gradient at zero to d TODO: is it really the goal to add ??
Definition: RegularHessianFactor.h:196
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
Definition: RegularHessianFactor.h:39
gtsam::SymmetricBlockMatrix::diagonal
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition: SymmetricBlockMatrix.h:147
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:137
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
Definition: RegularHessianFactor.h:68
offsets
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set offsets
Definition: gnuplot_common_settings.hh:27
gtsam::RegularJacobianFactor
Definition: RegularJacobianFactor.h:32
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
Construct from a GaussianFactorGraph.
Definition: RegularHessianFactor.h:79
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Definition: RegularHessianFactor.h:112
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::RegularHessianFactor::MatrixD
Eigen::Matrix< double, D, D > MatrixD
Definition: RegularHessianFactor.h:33
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition: SymmetricBlockMatrix.h:152
pos
Definition: example-NearestNeighbor.cpp:32
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
j1
double j1(double x)
Definition: j1.c:174
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors)
Construct from a GaussianFactorGraph.
Definition: RegularHessianFactor.h:86


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:58