gtsam  4.1.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
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
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  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
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:295
JacobianFactor.h
gtsam::JacobianFactor::empty
bool empty() const override
Check if the factor is empty.
Definition: JacobianFactor.h:264
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition: RegularJacobianFactor.h:82
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor()
Default constructor.
Definition: RegularJacobianFactor.h:44
gtsam::RegularJacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
Expose base class gradientAtZero.
Definition: RegularJacobianFactor.h:128
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
VectorValues.h
Factor Graph Values.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition: RegularJacobianFactor.h:73
gtsam::JacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition: JacobianFactor.cpp:657
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition: JacobianFactor.cpp:709
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::Factor::size
size_t size() const
Definition: Factor.h:129
gtsam::RegularJacobianFactor::RegularJacobianFactor
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
gtsam::RegularJacobianFactor::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
double* Transpose Matrix-vector multiply, i.e.
Definition: RegularJacobianFactor.h:161
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition: RegularJacobianFactor.h:32
gtsam::RegularJacobianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition: RegularJacobianFactor.h:109
gtsam::RegularJacobianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Raw memory access version of gradientAtZero.
Definition: RegularJacobianFactor.h:133
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition: GaussianFactor.cpp:27
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct an n-ary factor.
Definition: RegularJacobianFactor.h:52
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194
gtsam::RegularJacobianFactor::operator*
Vector operator*(const double *x) const
double* Matrix-vector multiply, i.e.
Definition: RegularJacobianFactor.h:172
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42