LinearizedFactor.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 
18 #pragma once
19 
20 #include <vector>
21 #include <gtsam_unstable/dllexport.h>
25 
26 namespace gtsam {
27 
31 class GTSAM_UNSTABLE_EXPORT LinearizedGaussianFactor : public NonlinearFactor {
32 public:
36 
38  typedef std::shared_ptr<LinearizedGaussianFactor> shared_ptr;
39 
40 protected:
41 
44 
45 public:
46 
48  LinearizedGaussianFactor() = default;
49 
54  LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points);
55 
56  ~LinearizedGaussianFactor() override = default;
57 
58  // access functions
59  const Values& linearizationPoint() const { return lin_points_; }
60 
61 private:
62 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
63 
64  friend class boost::serialization::access;
65  template<class ARCHIVE>
66  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
67  ar & boost::serialization::make_nvp("LinearizedGaussianFactor",
68  boost::serialization::base_object<Base>(*this));
69  ar & BOOST_SERIALIZATION_NVP(lin_points_);
70  }
71 #endif
72 
73 };
74 
79 class GTSAM_UNSTABLE_EXPORT LinearizedJacobianFactor : public LinearizedGaussianFactor {
80 
81 public:
85 
87  typedef std::shared_ptr<LinearizedJacobianFactor> shared_ptr;
88 
93 
94 protected:
95 
96 // // store components of a jacobian factor
97 // typedef std::map<Key, Matrix> KeyMatrixMap;
98 // KeyMatrixMap matrices_;
99 // Vector b_;
100 
101  VerticalBlockMatrix Ab_; // the block view of the full matrix
102 
103 public:
104 
107 
112  LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, const Values& lin_points);
113 
115 
118  return std::static_pointer_cast<gtsam::NonlinearFactor>(
120 
121  // Testable
122 
124  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
125 
127  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
128 
129  // access functions
130  const constBVector b() const { return Ab_(size()).col(0); }
131  const constABlock A() const { return Ab_.range(0, size()); }
132  const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); }
133 
135  size_t dim() const override { return Ab_.rows(); }
136 
138  double error(const Values& c) const override;
139 
145  std::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
146 
148  Vector error_vector(const Values& c) const;
149 
150 private:
151 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
152  friend class boost::serialization::access;
154  template<class ARCHIVE>
155  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
156  ar & boost::serialization::make_nvp("LinearizedJacobianFactor",
157  boost::serialization::base_object<Base>(*this));
158  ar & BOOST_SERIALIZATION_NVP(Ab_);
159  }
160 #endif
161 };
162 
164 template<>
165 struct traits<LinearizedJacobianFactor> : public Testable<LinearizedJacobianFactor> {
166 };
167 
172 class GTSAM_UNSTABLE_EXPORT LinearizedHessianFactor : public LinearizedGaussianFactor {
173 
174 public:
178 
180  typedef std::shared_ptr<LinearizedHessianFactor> shared_ptr;
181 
185 
188 
189 protected:
190 
192 
194 public:
195 
198 
204  LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, const Values& lin_points);
205 
207 
210  return std::static_pointer_cast<gtsam::NonlinearFactor>(
212 
213  // Testable
214 
216  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
217 
219  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
220 
224  double constantTerm() const {
225  const auto block = info_.diagonalBlock(size());
226  return block(0, 0);
227  }
228 
234  return info_.aboveDiagonalRange(j - begin(), size(), size(), size() + 1).col(0);
235  }
236 
240  return info_.aboveDiagonalRange(0, size(), size(), size() + 1).col(0);
241  }
242 
254  const DenseIndex J1 = j1 - begin();
255  const DenseIndex J2 = j2 - begin();
256  return info_.block(J1, J2);
257  }
258 
264  return info_.selfadjointView(0, size());
265  }
266 
268  size_t dim() const override { return info_.rows() - 1; }
269 
271  double error(const Values& c) const override;
272 
278  std::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
279 
280 private:
282 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
283  friend class boost::serialization::access;
284  template<class ARCHIVE>
285  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
286  ar & boost::serialization::make_nvp("LinearizedHessianFactor",
287  boost::serialization::base_object<Base>(*this));
288  ar & BOOST_SERIALIZATION_NVP(info_);
289  }
290 #endif
291 };
292 
294 template<>
295 struct traits<LinearizedHessianFactor> : public Testable<LinearizedHessianFactor> {
296 };
297 
298 } // \namespace aspn
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::LinearizedJacobianFactor::Base
LinearizedGaussianFactor Base
Definition: LinearizedFactor.h:83
gtsam::LinearizedJacobianFactor
Definition: LinearizedFactor.h:79
Eigen::Block
Expression of a fixed-size or dynamic-size block.
Definition: Block.h:103
gtsam::LinearizedJacobianFactor::A
const constABlock A(Key key) const
Definition: LinearizedFactor.h:132
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::LinearizedHessianFactor::constColumn
SymmetricBlockMatrix::constBlock::ColXpr constColumn
A column containing the linear term h (const version)
Definition: LinearizedFactor.h:187
gtsam::LinearizedHessianFactor::Block
SymmetricBlockMatrix::Block Block
A block from the Hessian matrix.
Definition: LinearizedFactor.h:183
gtsam::LinearizedHessianFactor::info_
SymmetricBlockMatrix info_
Definition: LinearizedFactor.h:191
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::LinearizedHessianFactor::dim
size_t dim() const override
Definition: LinearizedFactor.h:268
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
Definition: VerticalBlockMatrix.h:130
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition: SymmetricBlockMatrix.h:158
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::HessianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:109
Eigen::SelfAdjointView
Expression of a selfadjoint matrix from a triangular part of a dense matrix.
Definition: SelfAdjointView.h:49
gtsam::LinearizedGaussianFactor::Base
NonlinearFactor Base
Definition: LinearizedFactor.h:34
gtsam::LinearizedJacobianFactor::This
LinearizedJacobianFactor This
Definition: LinearizedFactor.h:84
gtsam::LinearizedJacobianFactor::dim
size_t dim() const override
Definition: LinearizedFactor.h:135
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
constBlock aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
Get a range [i,j) from the matrix. Indices are in block units.
Definition: SymmetricBlockMatrix.h:172
block
m m block(1, 0, 2, 2)<< 4
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
size
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
gtsam::LinearizedHessianFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: LinearizedFactor.h:209
gtsam::LinearizedHessianFactor::constBlock
SymmetricBlockMatrix::constBlock constBlock
A block from the Hessian matrix (const version)
Definition: LinearizedFactor.h:184
gtsam::LinearizedJacobianFactor::constBVector
VerticalBlockMatrix::constBlock::ConstColXpr constBVector
Definition: LinearizedFactor.h:92
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::LinearizedHessianFactor::squaredTerm
Matrix squaredTerm(const_iterator j1, const_iterator j2) const
Definition: LinearizedFactor.h:253
gtsam::LinearizedHessianFactor::~LinearizedHessianFactor
~LinearizedHessianFactor() override
Definition: LinearizedFactor.h:206
gtsam::LinearizedHessianFactor::Column
SymmetricBlockMatrix::Block::ColXpr Column
A column containing the linear term h.
Definition: LinearizedFactor.h:186
gtsam::LinearizedJacobianFactor::Ab_
VerticalBlockMatrix Ab_
Definition: LinearizedFactor.h:101
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
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
gtsam::LinearizedHessianFactor::squaredTerm
Eigen::SelfAdjointView< constBlock, Eigen::Upper > squaredTerm() const
Definition: LinearizedFactor.h:263
gtsam::LinearizedGaussianFactor::shared_ptr
std::shared_ptr< LinearizedGaussianFactor > shared_ptr
Definition: LinearizedFactor.h:38
gtsam::LinearizedGaussianFactor::This
LinearizedGaussianFactor This
Definition: LinearizedFactor.h:35
gtsam::LinearizedHessianFactor::linearTerm
constColumn linearTerm(const_iterator j) const
Definition: LinearizedFactor.h:233
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::LinearizedHessianFactor
Definition: LinearizedFactor.h:172
gtsam::LinearizedGaussianFactor
Definition: LinearizedFactor.h:31
gtsam::LinearizedHessianFactor::linearTerm
constColumn linearTerm() const
Definition: LinearizedFactor.h:239
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: SymmetricBlockMatrix.h:116
gtsam::equals
Definition: Testable.h:112
key
const gtsam::Symbol key('X', 0)
ColXpr
Block< Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ColXpr
Definition: BlockMethods.h:14
JacobianFactor.h
NonlinearFactor.h
Non-linear factor base classes.
ConstColXpr
const typedef Block< const Derived, internal::traits< Derived >::RowsAtCompileTime, 1, !IsRowMajor > ConstColXpr
Definition: BlockMethods.h:15
gtsam::LinearizedJacobianFactor::b
const constBVector b() const
Definition: LinearizedFactor.h:130
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam
traits
Definition: SFMdata.h:40
gtsam::LinearizedJacobianFactor::~LinearizedJacobianFactor
~LinearizedJacobianFactor() override
Definition: LinearizedFactor.h:114
gtsam::Testable
Definition: Testable.h:152
error
static double error
Definition: testRot3.cpp:37
gtsam::traits
Definition: Group.h:36
gtsam::LinearizedHessianFactor::Base
LinearizedGaussianFactor Base
Definition: LinearizedFactor.h:176
gtsam::Values
Definition: Values.h:65
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
gtsam::LinearizedJacobianFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: LinearizedFactor.h:117
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::LinearizedJacobianFactor::BVector
VerticalBlockMatrix::Block::ColXpr BVector
Definition: LinearizedFactor.h:91
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition: SymmetricBlockMatrix.h:137
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::LinearizedHessianFactor::This
LinearizedHessianFactor This
Definition: LinearizedFactor.h:177
gtsam::LinearizedJacobianFactor::shared_ptr
std::shared_ptr< LinearizedJacobianFactor > shared_ptr
Definition: LinearizedFactor.h:87
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:115
gtsam::LinearizedHessianFactor::constantTerm
double constantTerm() const
Definition: LinearizedFactor.h:224
gtsam::LinearizedJacobianFactor::constABlock
VerticalBlockMatrix::constBlock constABlock
Definition: LinearizedFactor.h:90
gtsam::LinearizedJacobianFactor::A
const constABlock A() const
Definition: LinearizedFactor.h:131
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::JacobianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:97
gtsam::SymmetricBlockMatrix::block
Matrix block(DenseIndex I, DenseIndex J) const
Definition: SymmetricBlockMatrix.cpp:60
gtsam::LinearizedJacobianFactor::ABlock
VerticalBlockMatrix::Block ABlock
Definition: LinearizedFactor.h:89
gtsam::LinearizedGaussianFactor::lin_points_
Values lin_points_
Definition: LinearizedFactor.h:43
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:53
j1
double j1(double x)
Definition: j1.c:174
gtsam::LinearizedGaussianFactor::linearizationPoint
const Values & linearizationPoint() const
Definition: LinearizedFactor.h:59
gtsam::LinearizedHessianFactor::shared_ptr
std::shared_ptr< LinearizedHessianFactor > shared_ptr
Definition: LinearizedFactor.h:180


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:07