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 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
28 #include <boost/serialization/version.hpp>
29 #include <boost/serialization/split_member.hpp>
30 #endif
31 
32 namespace gtsam {
33 
34  // Forward declarations
35  class HessianFactor;
36  class VariableSlots;
37  class GaussianFactorGraph;
38  class GaussianConditional;
39  class HessianFactor;
40  class VectorValues;
41  class Ordering;
42  class JacobianFactor;
43 
49  GTSAM_EXPORT std::pair<std::shared_ptr<GaussianConditional>, std::shared_ptr<JacobianFactor> >
50  EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
51 
91  class GTSAM_EXPORT JacobianFactor : public GaussianFactor
92  {
93  public:
94 
95  typedef JacobianFactor This;
96  typedef GaussianFactor Base;
97  typedef std::shared_ptr<This> shared_ptr;
98 
103 
104  protected:
105 
106  VerticalBlockMatrix Ab_; // the block view of the full matrix
107  noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
108 
109  public:
110 
112  explicit JacobianFactor(const GaussianFactor& gf);
113 
115  JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {}
116 
118  explicit JacobianFactor(const HessianFactor& hf);
119 
121  JacobianFactor();
122 
124  explicit JacobianFactor(const Vector& b_in);
125 
127  JacobianFactor(Key i1, const Matrix& A1,
128  const Vector& b, const SharedDiagonal& model = SharedDiagonal());
129 
131  JacobianFactor(Key i1, const Matrix& A1,
132  Key i2, const Matrix& A2,
133  const Vector& b, const SharedDiagonal& model = SharedDiagonal());
134 
136  JacobianFactor(Key i1, const Matrix& A1, Key i2,
137  const Matrix& A2, Key i3, const Matrix& A3,
138  const Vector& b, const SharedDiagonal& model = SharedDiagonal());
139 
143  template<typename TERMS>
144  JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());
145 
150  template<typename KEYS>
152  const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
153 
158  explicit JacobianFactor(
159  const GaussianFactorGraph& graph);
160 
165  explicit JacobianFactor(
166  const GaussianFactorGraph& graph,
167  const VariableSlots& p_variableSlots);
168 
173  explicit JacobianFactor(
174  const GaussianFactorGraph& graph,
175  const Ordering& ordering);
176 
181  explicit JacobianFactor(
182  const GaussianFactorGraph& graph,
183  const Ordering& ordering,
184  const VariableSlots& p_variableSlots);
185 
187  ~JacobianFactor() override {}
188 
191  return std::static_pointer_cast<GaussianFactor>(
192  std::make_shared<JacobianFactor>(*this));
193  }
194 
195  // Implementing Testable interface
196  void print(const std::string& s = "",
197  const KeyFormatter& formatter = DefaultKeyFormatter) const override;
198  bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
199 
200  Vector unweighted_error(const VectorValues& c) const;
201  Vector error_vector(const VectorValues& c) const;
203  using GaussianFactor::error;
205 
207  double error(const VectorValues& c) const override;
208 
217  Matrix augmentedInformation() const override;
218 
222  Matrix information() const override;
223 
225  using Base::hessianDiagonal;
226 
228  void hessianDiagonalAdd(VectorValues& d) const override;
229 
231  void hessianDiagonal(double* d) const override;
232 
234  std::map<Key,Matrix> hessianBlockDiagonal() const override;
235 
239  std::pair<Matrix, Vector> jacobian() const override;
240 
244  std::pair<Matrix, Vector> jacobianUnweighted() const;
245 
249  Matrix augmentedJacobian() const override;
250 
254  Matrix augmentedJacobianUnweighted() const;
255 
257  const VerticalBlockMatrix& matrixObject() const { return Ab_; }
258 
261 
267  GaussianFactor::shared_ptr negate() const override;
268 
270  bool isConstrained() const {
271  return model_ && model_->isConstrained();
272  }
273 
277  DenseIndex getDim(const_iterator variable) const override {
278  return Ab_(variable - begin()).cols();
279  }
280 
284  size_t rows() const { return Ab_.rows(); }
285 
289  size_t cols() const { return Ab_.cols(); }
290 
292  const SharedDiagonal& get_model() const { return model_; }
293 
295  SharedDiagonal& get_model() { return model_; }
296 
298  const constBVector getb() const { return Ab_(size()).col(0); }
299 
301  constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
302 
304  constABlock getA() const { return Ab_.range(0, size()); }
305 
307  BVector getb() { return Ab_(size()).col(0); }
308 
310  ABlock getA(iterator variable) { return Ab_(variable - begin()); }
311 
313  ABlock getA() { return Ab_.range(0, size()); }
314 
320  void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
321 
323  Vector operator*(const VectorValues& x) const;
324 
327  void transposeMultiplyAdd(double alpha, const Vector& e,
328  VectorValues& x) const;
329 
331  void multiplyHessianAdd(double alpha, const VectorValues& x,
332  VectorValues& y) const override;
333 
342  void multiplyHessianAdd(double alpha, const double* x, double* y,
343  const std::vector<size_t>& accumulatedDims) const;
344 
346  VectorValues gradientAtZero() const override;
347 
349  void gradientAtZero(double* d) const override;
350 
352  Vector gradient(Key key, const VectorValues& x) const override;
353 
355  JacobianFactor whiten() const;
356 
358  std::pair<std::shared_ptr<GaussianConditional>, shared_ptr>
359  eliminate(const Ordering& keys);
360 
362  void setModel(bool anyConstrained, const Vector& sigmas);
363 
375  friend GTSAM_EXPORT std::pair<std::shared_ptr<GaussianConditional>, shared_ptr>
376  EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
377 
385  std::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
386 
387  protected:
388 
390  template<typename TERMS>
391  void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
392 
393  private:
394 
399  void JacobianFactorHelper(
400  const GaussianFactorGraph& graph,
401  const FastVector<VariableSlots::const_iterator>& orderedSlots);
402 
409  template<class KEYS, class DIMENSIONS>
410  JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
411  const SharedDiagonal& model = SharedDiagonal()) :
412  Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
413  }
414 
415  // be very selective on who can access these private methods:
416  template<typename T> friend class ExpressionFactor;
417 
418 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
419 
420  friend class boost::serialization::access;
421  template<class ARCHIVE>
422  void save(ARCHIVE & ar, const unsigned int version) const {
423  // TODO(fan): This is a hack for Boost < 1.66
424  // We really need to introduce proper versioning in the archives
425  // As otherwise this will not read objects serialized by older
426  // versions of GTSAM
427  ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
428  ar << BOOST_SERIALIZATION_NVP(Ab_);
429  bool model_null = false;
430  if(model_.get() == nullptr) {
431  model_null = true;
432  ar << boost::serialization::make_nvp("model_null", model_null);
433  } else {
434  ar << boost::serialization::make_nvp("model_null", model_null);
435  ar << BOOST_SERIALIZATION_NVP(model_);
436  }
437  }
438 
439  template<class ARCHIVE>
440  void load(ARCHIVE & ar, const unsigned int version) {
441  // invoke serialization of the base class
442  ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
443  ar >> BOOST_SERIALIZATION_NVP(Ab_);
444  if (version < 1) {
445  ar >> BOOST_SERIALIZATION_NVP(model_);
446  } else {
447  bool model_null;
448  ar >> BOOST_SERIALIZATION_NVP(model_null);
449  if (!model_null) {
450  ar >> BOOST_SERIALIZATION_NVP(model_);
451  }
452  }
453  }
454 
455  BOOST_SERIALIZATION_SPLIT_MEMBER()
456 #endif
457  }; // JacobianFactor
459 template<>
460 struct traits<JacobianFactor> : public Testable<JacobianFactor> {
461 };
462 
463 } // \ namespace gtsam
464 
465 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
466 BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
467 #endif
468 
470 
471 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
Matrix3f m
static Matrix A1
DenseIndex cols() const
Column size.
void save(const Matrix &A, const string &s, const string &filename)
Definition: Matrix.cpp:166
Scalar * y
const Block< const Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ConstColXpr
Definition: BlockMethods.h:15
const VerticalBlockMatrix & matrixObject() const
noiseModel::Diagonal::shared_ptr model
DenseIndex rows() const
Row size.
A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and GaussianConditional.
noiseModel::Diagonal::shared_ptr model_
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
A factor with a quadratic error function - a Gaussian.
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
const GaussianFactorGraph factors
Block< Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ColXpr
Definition: BlockMethods.h:14
ABlock::ColXpr BVector
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
NonlinearFactorGraph graph
JacobianFactor(const JacobianFactor &jf)
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
static enum @1107 ordering
SharedDiagonal & get_model()
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
const KeyFormatter & formatter
Included from all GTSAM files.
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
bool isConstrained() const
else if n * info
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
Eigen::VectorXd Vector
Definition: Vector.h:38
GaussianFactor::shared_ptr clone() const override
constABlock::ConstColXpr constBVector
VerticalBlockMatrix::constBlock constABlock
const constBVector getb() const
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
DenseIndex getDim(const_iterator variable) const override
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
VerticalBlockMatrix::Block ABlock
JacobianFactor(const KEYS &keys, const DIMENSIONS &dims, DenseIndex m, const SharedDiagonal &model=SharedDiagonal())
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
const G & b
Definition: Group.h:86
std::shared_ptr< This > shared_ptr
shared_ptr to this class
ABlock getA(iterator variable)
Block range(DenseIndex startBlock, DenseIndex endBlock)
VerticalBlockMatrix & matrixObject()
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
constABlock getA() const
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
static EIGEN_DEPRECATED const end_t end
JacobianFactor This
Typedef to this class.
A Gaussian factor using the canonical parameters (information form)
VerticalBlockMatrix Ab_
static double error
Definition: testRot3.cpp:37
const G double tol
Definition: Group.h:86
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
GaussianFactor Base
Typedef to base class.
const SharedDiagonal & get_model() const
virtual double error(const VectorValues &c) const
constABlock getA(const_iterator variable) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:27