gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
JacobianFactor.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
26
27#include <boost/make_shared.hpp>
28#include <boost/serialization/version.hpp>
29#include <boost/serialization/split_member.hpp>
30
31namespace gtsam {
32
33 // Forward declarations
34 class HessianFactor;
35 class VariableSlots;
36 class GaussianFactorGraph;
38 class HessianFactor;
39 class VectorValues;
40 class Ordering;
41 class JacobianFactor;
42
48 GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
49 EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
50
90 class GTSAM_EXPORT JacobianFactor : public GaussianFactor
91 {
92 public:
93
96 typedef boost::shared_ptr<This> shared_ptr;
97
98 typedef VerticalBlockMatrix::Block ABlock;
99 typedef VerticalBlockMatrix::constBlock constABlock;
100 typedef ABlock::ColXpr BVector;
101 typedef constABlock::ConstColXpr constBVector;
102
103 protected:
104
105 VerticalBlockMatrix Ab_; // the block view of the full matrix
106 noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
107
108 public:
109
111 explicit JacobianFactor(const GaussianFactor& gf);
112
114 JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {}
115
117 explicit JacobianFactor(const HessianFactor& hf);
118
121
123 explicit JacobianFactor(const Vector& b_in);
124
126 JacobianFactor(Key i1, const Matrix& A1,
127 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
128
130 JacobianFactor(Key i1, const Matrix& A1,
131 Key i2, const Matrix& A2,
132 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
133
135 JacobianFactor(Key i1, const Matrix& A1, Key i2,
136 const Matrix& A2, Key i3, const Matrix& A3,
137 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
138
142 template<typename TERMS>
143 JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());
144
149 template<typename KEYS>
151 const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
152
157 explicit JacobianFactor(
158 const GaussianFactorGraph& graph);
159
164 explicit JacobianFactor(
165 const GaussianFactorGraph& graph,
166 const VariableSlots& p_variableSlots);
167
172 explicit JacobianFactor(
173 const GaussianFactorGraph& graph,
174 const Ordering& ordering);
175
180 explicit JacobianFactor(
181 const GaussianFactorGraph& graph,
182 const Ordering& ordering,
183 const VariableSlots& p_variableSlots);
184
186 ~JacobianFactor() override {}
187
190 return boost::static_pointer_cast<GaussianFactor>(
191 boost::make_shared<JacobianFactor>(*this));
192 }
193
194 // Implementing Testable interface
195 void print(const std::string& s = "",
196 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
197 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
198
199 Vector unweighted_error(const VectorValues& c) const;
200 Vector error_vector(const VectorValues& c) const;
203 using GaussianFactor::error;
204
206 double error(const VectorValues& c) const override;
207
216 Matrix augmentedInformation() const override;
217
221 Matrix information() const override;
222
224 using Base::hessianDiagonal;
225
227 void hessianDiagonalAdd(VectorValues& d) const override;
228
230 void hessianDiagonal(double* d) const override;
231
233 std::map<Key,Matrix> hessianBlockDiagonal() const override;
234
238 std::pair<Matrix, Vector> jacobian() const override;
239
243 std::pair<Matrix, Vector> jacobianUnweighted() const;
244
248 Matrix augmentedJacobian() const override;
249
253 Matrix augmentedJacobianUnweighted() const;
254
256 const VerticalBlockMatrix& matrixObject() const { return Ab_; }
257
260
266 GaussianFactor::shared_ptr negate() const override;
267
269 bool isConstrained() const {
270 return model_ && model_->isConstrained();
271 }
272
276 DenseIndex getDim(const_iterator variable) const override {
277 return Ab_(variable - begin()).cols();
278 }
279
283 size_t rows() const { return Ab_.rows(); }
284
288 size_t cols() const { return Ab_.cols(); }
289
291 const SharedDiagonal& get_model() const { return model_; }
292
294 SharedDiagonal& get_model() { return model_; }
295
297 const constBVector getb() const { return Ab_(size()).col(0); }
298
300 constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
301
303 constABlock getA() const { return Ab_.range(0, size()); }
304
306 BVector getb() { return Ab_(size()).col(0); }
307
309 ABlock getA(iterator variable) { return Ab_(variable - begin()); }
310
312 ABlock getA() { return Ab_.range(0, size()); }
313
319 void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
320
322 Vector operator*(const VectorValues& x) const;
323
326 void transposeMultiplyAdd(double alpha, const Vector& e,
327 VectorValues& x) const;
328
330 void multiplyHessianAdd(double alpha, const VectorValues& x,
331 VectorValues& y) const override;
332
341 void multiplyHessianAdd(double alpha, const double* x, double* y,
342 const std::vector<size_t>& accumulatedDims) const;
343
345 VectorValues gradientAtZero() const override;
346
348 void gradientAtZero(double* d) const override;
349
351 Vector gradient(Key key, const VectorValues& x) const override;
352
354 JacobianFactor whiten() const;
355
357 std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
358 eliminate(const Ordering& keys);
359
361 void setModel(bool anyConstrained, const Vector& sigmas);
362
374 friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
375 EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
376
384 boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
385
386 protected:
387
389 template<typename TERMS>
390 void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
391
392 private:
393
398 void JacobianFactorHelper(
399 const GaussianFactorGraph& graph,
401
408 template<class KEYS, class DIMENSIONS>
409 JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
410 const SharedDiagonal& model = SharedDiagonal()) :
411 Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
412 }
413
414 // be very selective on who can access these private methods:
415 template<typename T> friend class ExpressionFactor;
416
418 friend class boost::serialization::access;
419 template<class ARCHIVE>
420 void save(ARCHIVE & ar, const unsigned int version) const {
421 // TODO(fan): This is a hack for Boost < 1.66
422 // We really need to introduce proper versioning in the archives
423 // As otherwise this will not read objects serialized by older
424 // versions of GTSAM
425 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
426 ar << BOOST_SERIALIZATION_NVP(Ab_);
427 bool model_null = false;
428 if(model_.get() == nullptr) {
429 model_null = true;
430 ar << boost::serialization::make_nvp("model_null", model_null);
431 } else {
432 ar << boost::serialization::make_nvp("model_null", model_null);
433 ar << BOOST_SERIALIZATION_NVP(model_);
434 }
435 }
436
437 template<class ARCHIVE>
438 void load(ARCHIVE & ar, const unsigned int version) {
439 // invoke serialization of the base class
440 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
441 ar >> BOOST_SERIALIZATION_NVP(Ab_);
442 if (version < 1) {
443 ar >> BOOST_SERIALIZATION_NVP(model_);
444 } else {
445 bool model_null;
446 ar >> BOOST_SERIALIZATION_NVP(model_null);
447 if (!model_null) {
448 ar >> BOOST_SERIALIZATION_NVP(model_);
449 }
450 }
451 }
452
453 BOOST_SERIALIZATION_SPLIT_MEMBER()
454 }; // JacobianFactor
456template<>
457struct traits<JacobianFactor> : public Testable<JacobianFactor> {
458};
459
460} // \ namespace gtsam
461
462BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
463
464#include <gtsam/linear/JacobianFactor-inl.h>
465
466
A matrix with column blocks of pre-defined sizes.
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
A factor with a quadratic error function - a Gaussian.
Included from all GTSAM files.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
Template to create a binary predicate.
Definition Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition VerticalBlockMatrix.h:130
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
KeyVector::iterator iterator
Iterator over keys.
Definition Factor.h:77
Definition Ordering.h:34
A combined factor is assembled as one block of rows for each component factor.
Definition VariableSlots.h:52
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
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:297
BVector getb()
Get a view of the r.h.s.
Definition JacobianFactor.h:306
const SharedDiagonal & get_model() const
get a copy of model
Definition JacobianFactor.h:291
JacobianFactor(const JacobianFactor &jf)
Copy constructor.
Definition JacobianFactor.h:114
JacobianFactor This
Typedef to this class.
Definition JacobianFactor.h:94
bool isConstrained() const
is noise model constrained ?
Definition JacobianFactor.h:269
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition JacobianFactor.h:303
GaussianFactor::shared_ptr clone() const override
Clone this JacobianFactor.
Definition JacobianFactor.h:189
ABlock getA(iterator variable)
Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version)
Definition JacobianFactor.h:309
SharedDiagonal & get_model()
get a copy of model (non-const version)
Definition JacobianFactor.h:294
DenseIndex getDim(const_iterator variable) const override
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition JacobianFactor.h:276
VerticalBlockMatrix & matrixObject()
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:259
ABlock getA()
Get a view of the A matrix.
Definition JacobianFactor.h:312
GaussianFactor Base
Typedef to base class.
Definition JacobianFactor.h:95
~JacobianFactor() override
Virtual destructor.
Definition JacobianFactor.h:186
const VerticalBlockMatrix & matrixObject() const
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:256
size_t rows() const
return the number of rows in the corresponding linear system
Definition JacobianFactor.h:283
size_t cols() const
return the number of columns in the corresponding linear system
Definition JacobianFactor.h:288
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
constABlock getA(const_iterator variable) const
Get a view of the A matrix for the variable pointed to by the given key iterator.
Definition JacobianFactor.h:300
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
In Gaussian factors, the error function returns either the negative log-likelihood,...
is the normalization constant.
The Factor::error simply extracts the.