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