gtsam  4.1.0
gtsam
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 
24 #include <gtsam/global_includes.h>
26 
27 #include <boost/make_shared.hpp>
28 #include <boost/serialization/version.hpp>
29 #include <boost/serialization/split_member.hpp>
30 
31 namespace gtsam {
32 
33  // Forward declarations
34  class HessianFactor;
35  class VariableSlots;
36  class GaussianFactorGraph;
37  class GaussianConditional;
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 
94  typedef JacobianFactor This;
95  typedef GaussianFactor Base;
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 
120  JacobianFactor();
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  virtual ~JacobianFactor() {}
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;
201  double error(const VectorValues& c) const override;
211  Matrix augmentedInformation() const override;
212 
216  Matrix information() const override;
217 
219  using Base::hessianDiagonal;
220 
222  void hessianDiagonalAdd(VectorValues& d) const override;
223 
225  void hessianDiagonal(double* d) const override;
226 
228  std::map<Key,Matrix> hessianBlockDiagonal() const override;
229 
233  std::pair<Matrix, Vector> jacobian() const override;
234 
238  std::pair<Matrix, Vector> jacobianUnweighted() const;
239 
243  Matrix augmentedJacobian() const override;
244 
248  Matrix augmentedJacobianUnweighted() const;
249 
251  const VerticalBlockMatrix& matrixObject() const { return Ab_; }
252 
255 
261  GaussianFactor::shared_ptr negate() const override;
262 
264  bool empty() const override { return size() == 0 /*|| rows() == 0*/; }
265 
267  bool isConstrained() const {
268  return model_ && model_->isConstrained();
269  }
270 
274  DenseIndex getDim(const_iterator variable) const override {
275  return Ab_(variable - begin()).cols();
276  }
277 
281  size_t rows() const { return Ab_.rows(); }
282 
286  size_t cols() const { return Ab_.cols(); }
287 
289  const SharedDiagonal& get_model() const { return model_; }
290 
292  SharedDiagonal& get_model() { return model_; }
293 
295  const constBVector getb() const { return Ab_(size()).col(0); }
296 
298  constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
299 
301  constABlock getA() const { return Ab_.range(0, size()); }
302 
304  BVector getb() { return Ab_(size()).col(0); }
305 
307  ABlock getA(iterator variable) { return Ab_(variable - begin()); }
308 
310  ABlock getA() { return Ab_.range(0, size()); }
311 
317  void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
318 
320  Vector operator*(const VectorValues& x) const;
321 
324  void transposeMultiplyAdd(double alpha, const Vector& e,
325  VectorValues& x) const;
326 
328  void multiplyHessianAdd(double alpha, const VectorValues& x,
329  VectorValues& y) const override;
330 
339  void multiplyHessianAdd(double alpha, const double* x, double* y,
340  const std::vector<size_t>& accumulatedDims) const;
341 
343  VectorValues gradientAtZero() const override;
344 
346  void gradientAtZero(double* d) const override;
347 
349  Vector gradient(Key key, const VectorValues& x) const override;
350 
352  JacobianFactor whiten() const;
353 
355  std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
356  eliminate(const Ordering& keys);
357 
359  void setModel(bool anyConstrained, const Vector& sigmas);
360 
372  friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
373  EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
374 
382  boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
383 
384  protected:
385 
387  template<typename TERMS>
388  void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
389 
390  private:
391 
396  void JacobianFactorHelper(
397  const GaussianFactorGraph& graph,
398  const FastVector<VariableSlots::const_iterator>& orderedSlots);
399 
406  template<class KEYS, class DIMENSIONS>
407  JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
408  const SharedDiagonal& model = SharedDiagonal()) :
409  Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
410  }
411 
412  // be very selective on who can access these private methods:
413  template<typename T> friend class ExpressionFactor;
414 
416  friend class boost::serialization::access;
417  template<class ARCHIVE>
418  void save(ARCHIVE & ar, const unsigned int version) const {
419  // TODO(fan): This is a hack for Boost < 1.66
420  // We really need to introduce proper versioning in the archives
421  // As otherwise this will not read objects serialized by older
422  // versions of GTSAM
423  ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
424  ar << BOOST_SERIALIZATION_NVP(Ab_);
425  bool model_null = false;
426  if(model_.get() == nullptr) {
427  model_null = true;
428  ar << boost::serialization::make_nvp("model_null", model_null);
429  } else {
430  ar << boost::serialization::make_nvp("model_null", model_null);
431  ar << BOOST_SERIALIZATION_NVP(model_);
432  }
433  }
434 
435  template<class ARCHIVE>
436  void load(ARCHIVE & ar, const unsigned int version) {
437  // invoke serialization of the base class
438  ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
439  ar >> BOOST_SERIALIZATION_NVP(Ab_);
440  if (version < 1) {
441  ar >> BOOST_SERIALIZATION_NVP(model_);
442  } else {
443  bool model_null;
444  ar >> BOOST_SERIALIZATION_NVP(model_null);
445  if (!model_null) {
446  ar >> BOOST_SERIALIZATION_NVP(model_);
447  }
448  }
449  }
450 
451  BOOST_SERIALIZATION_SPLIT_MEMBER()
452  }; // JacobianFactor
454 template<>
455 struct traits<JacobianFactor> : public Testable<JacobianFactor> {
456 };
457 
458 } // \ namespace gtsam
459 
460 BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
461 
462 #include <gtsam/linear/JacobianFactor-inl.h>
463 
464 
gtsam::JacobianFactor::getA
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition: JacobianFactor.h:301
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition: JacobianFactor.h:295
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Ordering
Definition: Ordering.h:34
gtsam::JacobianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition: JacobianFactor.h:95
gtsam::JacobianFactor::empty
bool empty() const override
Check if the factor is empty.
Definition: JacobianFactor.h:264
gtsam::JacobianFactor::get_model
SharedDiagonal & get_model()
get a copy of model (non-const version)
Definition: JacobianFactor.h:292
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
gtsam::JacobianFactor::getA
ABlock getA()
Get a view of the A matrix.
Definition: JacobianFactor.h:310
gtsam::EliminateQR
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:797
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition: VerticalBlockMatrix.h:129
gtsam::JacobianFactor::matrixObject
VerticalBlockMatrix & matrixObject()
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition: JacobianFactor.h:254
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::JacobianFactor::getDim
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:274
gtsam::JacobianFactor::getA
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:307
gtsam::JacobianFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone this JacobianFactor.
Definition: JacobianFactor.h:189
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
global_includes.h
Included from all GTSAM files.
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::JacobianFactor::rows
size_t rows() const
return the number of rows in the corresponding linear system
Definition: JacobianFactor.h:281
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
get a copy of model
Definition: JacobianFactor.h:289
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const JacobianFactor &jf)
Copy constructor.
Definition: JacobianFactor.h:114
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::JacobianFactor::This
JacobianFactor This
Typedef to this class.
Definition: JacobianFactor.h:94
gtsam::VariableSlots
A combined factor is assembled as one block of rows for each component factor.
Definition: VariableSlots.h:52
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:101
gtsam::KeyFormatter
boost::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
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::JacobianFactor::getb
BVector getb()
Get a view of the r.h.s.
Definition: JacobianFactor.h:304
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
gtsam::JacobianFactor::cols
size_t cols() const
return the number of columns in the corresponding linear system
Definition: JacobianFactor.h:286
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:45
gtsam::save
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:166
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition: VerticalBlockMatrix.h:117
gtsam::Factor::iterator
KeyVector::iterator iterator
Iterator over keys.
Definition: Factor.h:64
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
gtsam::JacobianFactor::~JacobianFactor
virtual ~JacobianFactor()
Virtual destructor.
Definition: JacobianFactor.h:186
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes.
gtsam::JacobianFactor::getA
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:298
gtsam::JacobianFactor::matrixObject
const VerticalBlockMatrix & matrixObject() const
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition: JacobianFactor.h:251
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
is noise model constrained ?
Definition: JacobianFactor.h:267
NoiseModel.h
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42