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 
319  ABlock getA(const Key& key) { return Ab_(find(key) - begin()); }
320 
326  void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
327 
329  Vector operator*(const VectorValues& x) const;
330 
333  void transposeMultiplyAdd(double alpha, const Vector& e,
334  VectorValues& x) const;
335 
337  void multiplyHessianAdd(double alpha, const VectorValues& x,
338  VectorValues& y) const override;
339 
348  void multiplyHessianAdd(double alpha, const double* x, double* y,
349  const std::vector<size_t>& accumulatedDims) const;
350 
352  VectorValues gradientAtZero() const override;
353 
355  void gradientAtZero(double* d) const override;
356 
358  Vector gradient(Key key, const VectorValues& x) const override;
359 
361  JacobianFactor whiten() const;
362 
364  std::pair<std::shared_ptr<GaussianConditional>, shared_ptr>
365  eliminate(const Ordering& keys);
366 
368  void setModel(bool anyConstrained, const Vector& sigmas);
369 
381  friend GTSAM_EXPORT std::pair<std::shared_ptr<GaussianConditional>, shared_ptr>
383 
391  std::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
392 
393  protected:
394 
396  template<typename TERMS>
397  void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
398 
399  private:
400 
405  void JacobianFactorHelper(
406  const GaussianFactorGraph& graph,
407  const FastVector<VariableSlots::const_iterator>& orderedSlots);
408 
415  template<class KEYS, class DIMENSIONS>
416  JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
417  const SharedDiagonal& model = SharedDiagonal()) :
418  Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
419  }
420 
421  // be very selective on who can access these private methods:
422  template<typename T> friend class ExpressionFactor;
423 
424 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
425 
426  friend class boost::serialization::access;
427  template<class ARCHIVE>
428  void save(ARCHIVE & ar, const unsigned int version) const {
429  // TODO(fan): This is a hack for Boost < 1.66
430  // We really need to introduce proper versioning in the archives
431  // As otherwise this will not read objects serialized by older
432  // versions of GTSAM
433  ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
434  ar << BOOST_SERIALIZATION_NVP(Ab_);
435  bool model_null = false;
436  if(model_.get() == nullptr) {
437  model_null = true;
438  ar << boost::serialization::make_nvp("model_null", model_null);
439  } else {
440  ar << boost::serialization::make_nvp("model_null", model_null);
441  ar << BOOST_SERIALIZATION_NVP(model_);
442  }
443  }
444 
445  template<class ARCHIVE>
446  void load(ARCHIVE & ar, const unsigned int version) {
447  // invoke serialization of the base class
448  ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
449  ar >> BOOST_SERIALIZATION_NVP(Ab_);
450  if (version < 1) {
451  ar >> BOOST_SERIALIZATION_NVP(model_);
452  } else {
453  bool model_null;
454  ar >> BOOST_SERIALIZATION_NVP(model_null);
455  if (!model_null) {
456  ar >> BOOST_SERIALIZATION_NVP(model_);
457  }
458  }
459  }
460 
461  BOOST_SERIALIZATION_SPLIT_MEMBER()
462 #endif
463  }; // JacobianFactor
465 template<>
466 struct traits<JacobianFactor> : public Testable<JacobianFactor> {
467 };
468 
469 } // \ namespace gtsam
470 
471 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
472 BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
473 #endif
474 
476 
477 
VectorValues
gtsam::GaussianFactor::error
virtual double error(const VectorValues &c) const
Definition: GaussianFactor.cpp:27
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
A3
static const double A3[]
Definition: expn.h:8
gtsam::VariableSlots
Definition: VariableSlots.h:51
gtsam::JacobianFactor::Ab_
VerticalBlockMatrix Ab_
Definition: JacobianFactor.h:106
alpha
RealScalar alpha
Definition: level1_cplx_impl.h:147
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
global_includes.h
Included from all GTSAM files.
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
Definition: JacobianFactor.h:292
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::JacobianFactor::getA
ABlock getA()
Definition: JacobianFactor.h:313
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
Definition: JacobianFactor.h:270
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::JacobianFactor::~JacobianFactor
~JacobianFactor() override
Definition: JacobianFactor.h:187
gtsam::JacobianFactor::clone
GaussianFactor::shared_ptr clone() const override
Definition: JacobianFactor.h:190
gtsam::GaussianFactor
Definition: GaussianFactor.h:38
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
Definition: VerticalBlockMatrix.h:130
gtsam::JacobianFactor::constABlock
VerticalBlockMatrix::constBlock constABlock
Definition: JacobianFactor.h:100
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:70
gtsam::JacobianFactor::getb
BVector getb()
Definition: JacobianFactor.h:307
gtsam::JacobianFactor::getA
ABlock getA(const Key &key)
Definition: JacobianFactor.h:319
iterator
Definition: pytypes.h:1460
gtsam::JacobianFactor::rows
size_t rows() const
Definition: JacobianFactor.h:284
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::JacobianFactor::getb
const constBVector getb() const
Definition: JacobianFactor.h:298
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::JacobianFactor::model_
noiseModel::Diagonal::shared_ptr model_
Definition: JacobianFactor.h:107
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::JacobianFactor::getA
ABlock getA(iterator variable)
Definition: JacobianFactor.h:310
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const KEYS &keys, const DIMENSIONS &dims, DenseIndex m, const SharedDiagonal &model=SharedDiagonal())
Definition: JacobianFactor.h:416
gtsam::JacobianFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Definition: JacobianFactor.h:277
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
GaussianConditional
gtsam::VectorValues
Definition: VectorValues.h:74
Eigen::internal::negate
T negate(const T &x)
Definition: packetmath_test_shared.h:24
gtsam::JacobianFactor::BVector
ABlock::ColXpr BVector
Definition: JacobianFactor.h:101
gtsam::KeyFormatter
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
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes. Used in JacobianFactor and GaussianConditional.
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
A2
static const double A2[]
Definition: expn.h:7
gtsam::JacobianFactor::get_model
SharedDiagonal & get_model()
Definition: JacobianFactor.h:295
info
else if n * info
Definition: 3rdparty/Eigen/lapack/cholesky.cpp:18
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const JacobianFactor &jf)
Definition: JacobianFactor.h:115
conf.version
version
Definition: gtsam/3rdparty/GeographicLib/python/doc/conf.py:67
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::JacobianFactor::This
JacobianFactor This
Typedef to this class.
Definition: JacobianFactor.h:95
gtsam::JacobianFactor::cols
size_t cols() const
Definition: JacobianFactor.h:289
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
ordering
static enum @1096 ordering
gtsam::equals
Definition: Testable.h:112
y
Scalar * y
Definition: level1_cplx_impl.h:124
key
const gtsam::Symbol key('X', 0)
ColXpr
Block< Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ColXpr
Definition: BlockMethods.h:14
ConstColXpr
const typedef Block< const Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ConstColXpr
Definition: BlockMethods.h:15
gtsam::b
const G & b
Definition: Group.h:79
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
Definition: Matrix.cpp:166
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam::JacobianFactor::constBVector
constABlock::ConstColXpr constBVector
Definition: JacobianFactor.h:102
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition: VerticalBlockMatrix.h:118
error
static double error
Definition: testRot3.cpp:37
NoiseModel.h
gtsam::traits
Definition: Group.h:36
i1
double i1(double x)
Definition: i1.c:150
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Definition: JacobianFactor.h:301
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
gtsam::JacobianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition: JacobianFactor.h:96
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
JacobianFactor-inl.h
A1
static const double A1[]
Definition: expn.h:6
JacobianFactor
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:115
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::JacobianFactor::ABlock
VerticalBlockMatrix::Block ABlock
Definition: JacobianFactor.h:99
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
gtsam::JacobianFactor::matrixObject
const VerticalBlockMatrix & matrixObject() const
Definition: JacobianFactor.h:257
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::JacobianFactor::getA
constABlock getA() const
Definition: JacobianFactor.h:304
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
Base
Definition: test_virtual_functions.cpp:156
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
gtsam::JacobianFactor::matrixObject
VerticalBlockMatrix & matrixObject()
Definition: JacobianFactor.h:260
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::ExpressionFactor
Definition: Expression.h:36
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:55