RegularJacobianFactor.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 
24 namespace gtsam {
25 
31 template<size_t D>
33 
34 private:
35 
36  // Use eigen magic to access raw memory
40 
41 public:
42 
45 
51  template<typename TERMS>
52  RegularJacobianFactor(const TERMS& terms, const Vector& b,
53  const SharedDiagonal& model = SharedDiagonal()) :
54  JacobianFactor(terms, b, model) {
55  }
56 
63  template<typename KEYS>
65  const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
66  SharedDiagonal()) :
67  JacobianFactor(keys, augmentedMatrix, sigmas) {
68  }
69 
71 
73  void multiplyHessianAdd(double alpha, const VectorValues& x,
74  VectorValues& y) const override {
76  }
77 
82  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
83  if (empty())
84  return;
85  Vector Ax = Vector::Zero(Ab_.rows());
86 
87  // Just iterate over all A matrices and multiply in correct config part
88  for (size_t pos = 0; pos < size(); ++pos)
89  Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
90 
91  // Deal with noise properly, need to Double* whiten as we are dividing by variance
92  if (model_) {
93  model_->whitenInPlace(Ax);
94  model_->whitenInPlace(Ax);
95  }
96 
97  // multiply with alpha
98  Ax *= alpha;
99 
100  // Again iterate over all A matrices and insert Ai^e into y
101  for (size_t pos = 0; pos < size(); ++pos)
102  DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
103  }
104 
107 
109  void hessianDiagonal(double* d) const override {
110  // Loop over all variables in the factor
111  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
112  // Get the diagonal block, and insert its diagonal
113  DVector dj;
114  for (size_t k = 0; k < D; ++k) {
115  if (model_) {
116  Vector column_k = Ab_(j).col(k);
117  column_k = model_->whiten(column_k);
118  dj(k) = dot(column_k, column_k);
119  } else {
120  dj(k) = Ab_(j).col(k).squaredNorm();
121  }
122  }
123  DMap(d + D * j) += dj;
124  }
125  }
126 
128  VectorValues gradientAtZero() const override {
130  }
131 
133  void gradientAtZero(double* d) const override {
134 
135  // Get vector b not weighted
136  Vector b = getb();
137 
138  // Whitening b
139  if (model_) {
140  b = model_->whiten(b);
141  b = model_->whiten(b);
142  }
143 
144  // Just iterate over all A matrices
145  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
146  DVector dj;
147  // gradient -= A'*b/sigma^2
148  // Computing with each column
149  for (size_t k = 0; k < D; ++k) {
150  Vector column_k = Ab_(j).col(k);
151  dj(k) = -1.0 * dot(b, column_k);
152  }
153  DMap(d + D * j) += dj;
154  }
155  }
156 
161  void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
162  Vector E = alpha * (model_ ? model_->whiten(e) : e);
163  // Just iterate over all A matrices and insert Ai^e into y
164  for (size_t pos = 0; pos < size(); ++pos)
165  DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
166  }
167 
172  Vector operator*(const double* x) const {
173  Vector Ax = Vector::Zero(Ab_.rows());
174  if (empty())
175  return Ax;
176 
177  // Just iterate over all A matrices and multiply in correct config part
178  for (size_t pos = 0; pos < size(); ++pos)
179  Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
180 
181  return model_ ? model_->whiten(Ax) : Ax;
182  }
183 
184 };
185 // end class RegularJacobianFactor
186 
187 }// end namespace gtsam
Vector operator*(const double *x) const
double* Matrix-vector multiply, i.e. y = A*x RAW memory access! Assumes keys start at 0 and go to M-1...
bool empty() const
Whether the factor is empty (involves zero variables).
Definition: Factor.h:130
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e. y += A&#39;*(A*x) RAW memory access! Assumes keys start at 0 and go...
Scalar * y
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
noiseModel::Diagonal::shared_ptr model
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
size_t size() const
Definition: Factor.h:159
DenseIndex rows() const
Row size.
noiseModel::Diagonal::shared_ptr model_
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:87
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Factor Graph Values.
VectorValues gradientAtZero() const override
Expose base class gradientAtZero.
Eigen::VectorXd Vector
Definition: Vector.h:38
const constBVector getb() const
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RegularJacobianFactor()
Default constructor.
const G & b
Definition: Group.h:86
Eigen::Matrix< double, D, 1 > DVector
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
VectorValues gradientAtZero() const override
A&#39;*b for Jacobian.
DiscreteKey E(5, 2)
Eigen::Map< const DVector > ConstDMap
RegularJacobianFactor(const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())
VerticalBlockMatrix Ab_
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
void gradientAtZero(double *d) const override
Raw memory access version of gradientAtZero.
The matrix class, also used for vectors and row-vectors.
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
std::ptrdiff_t j
void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
double* Transpose Matrix-vector multiply, i.e. x += A&#39;*e RAW memory access! Assumes keys start at 0 a...
RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:32