gtsam  4.0.0 gtsam
RegularJacobianFactor.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
24 namespace gtsam {
25
31 template<size_t D>
33
34 private:
35
36  // Use eigen magic to access raw memory
37  typedef Eigen::Matrix<double, D, 1> DVector;
38  typedef Eigen::Map<DVector> DMap;
39  typedef Eigen::Map<const DVector> ConstDMap;
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  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
74  VectorValues& y) const {
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
106  virtual VectorValues hessianDiagonal() const {
108  }
109
111  void hessianDiagonal(double* d) const {
112  // Loop over all variables in the factor
113  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
114  // Get the diagonal block, and insert its diagonal
115  DVector dj;
116  for (size_t k = 0; k < D; ++k) {
117  if (model_) {
118  Vector column_k = Ab_(j).col(k);
119  column_k = model_->whiten(column_k);
120  dj(k) = dot(column_k, column_k);
121  } else {
122  dj(k) = Ab_(j).col(k).squaredNorm();
123  }
124  }
125  DMap(d + D * j) += dj;
126  }
127  }
128
130  virtual VectorValues gradientAtZero() const {
132  }
133
135  void gradientAtZero(double* d) const {
136
137  // Get vector b not weighted
138  Vector b = getb();
139
140  // Whitening b
141  if (model_) {
142  b = model_->whiten(b);
143  b = model_->whiten(b);
144  }
145
146  // Just iterate over all A matrices
147  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
148  DVector dj;
150  // Computing with each column
151  for (size_t k = 0; k < D; ++k) {
152  Vector column_k = Ab_(j).col(k);
153  dj(k) = -1.0 * dot(b, column_k);
154  }
155  DMap(d + D * j) += dj;
156  }
157  }
158
163  void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
164  Vector E = alpha * (model_ ? model_->whiten(e) : e);
165  // Just iterate over all A matrices and insert Ai^e into y
166  for (size_t pos = 0; pos < size(); ++pos)
167  DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
168  }
169
174  Vector operator*(const double* x) const {
175  Vector Ax = Vector::Zero(Ab_.rows());
176  if (empty())
177  return Ax;
178
179  // Just iterate over all A matrices and multiply in correct config part
180  for (size_t pos = 0; pos < size(); ++pos)
181  Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
182
183  return model_ ? model_->whiten(Ax) : Ax;
184  }
185
186 };
187 // end class RegularJacobianFactor
188
189 }// end namespace gtsam
virtual bool empty() const
Check if the factor is empty.
Definition: JacobianFactor.h:235
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:162
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:63
virtual VectorValues hessianDiagonal() const
Expose base class hessianDiagonal.
Definition: RegularJacobianFactor.h:106
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:264
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const
y += alpha * A'*A*x
Definition: JacobianFactor.cpp:581
virtual VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition: JacobianFactor.cpp:469
Definition: RegularJacobianFactor.h:130
virtual void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const
y += alpha * A'*A*x
Definition: RegularJacobianFactor.h:73
Raw memory access version of gradientAtZero.
Definition: RegularJacobianFactor.h:135
void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
double* Transpose Matrix-vector multiply, i.e.
Definition: RegularJacobianFactor.h:163
void hessianDiagonal(double *d) const
Raw memory access version of hessianDiagonal.
Definition: RegularJacobianFactor.h:111
Definition: VerticalBlockMatrix.h:41
Vector operator *(const double *x) const
double* Matrix-vector multiply, i.e.
Definition: RegularJacobianFactor.h:174
RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct an n-ary factor.
Definition: RegularJacobianFactor.h:52
size_t size() const
Definition: Factor.h:129
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:87
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition: RegularJacobianFactor.h:82
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
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
Factor Graph Values.
RegularJacobianFactor(const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())
Constructor with arbitrary number keys, and where the augmented matrix is given all together instead ...
Definition: RegularJacobianFactor.h:64
RegularJacobianFactor()
Default constructor.
Definition: RegularJacobianFactor.h:44