gtsam  4.1.0
gtsam
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 boost::shared_ptr<LinearizedGaussianFactor> shared_ptr;
39 
40 protected:
41 
44 
45 public:
46 
49 
54  LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points);
55 
56  virtual ~LinearizedGaussianFactor() {};
57 
58  // access functions
59  const Values& linearizationPoint() const { return lin_points_; }
60 
61 private:
63  friend class boost::serialization::access;
64  template<class ARCHIVE>
65  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
66  ar & boost::serialization::make_nvp("LinearizedGaussianFactor",
67  boost::serialization::base_object<Base>(*this));
68  ar & BOOST_SERIALIZATION_NVP(lin_points_);
69  }
70 
71 };
72 
77 class GTSAM_UNSTABLE_EXPORT LinearizedJacobianFactor : public LinearizedGaussianFactor {
78 
79 public:
83 
85  typedef boost::shared_ptr<LinearizedJacobianFactor> shared_ptr;
86 
87  typedef VerticalBlockMatrix::Block ABlock;
88  typedef VerticalBlockMatrix::constBlock constABlock;
89  typedef VerticalBlockMatrix::Block::ColXpr BVector;
90  typedef VerticalBlockMatrix::constBlock::ConstColXpr constBVector;
91 
92 protected:
93 
94 // // store components of a jacobian factor
95 // typedef std::map<Key, Matrix> KeyMatrixMap;
96 // KeyMatrixMap matrices_;
97 // Vector b_;
98 
99  VerticalBlockMatrix Ab_; // the block view of the full matrix
100 
101 public:
102 
105 
110  LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, const Values& lin_points);
111 
112  virtual ~LinearizedJacobianFactor() {}
113 
115  gtsam::NonlinearFactor::shared_ptr clone() const override {
116  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
117  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
118 
119  // Testable
120 
122  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
123 
125  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
126 
127  // access functions
128  const constBVector b() const { return Ab_(size()).col(0); }
129  const constABlock A() const { return Ab_.range(0, size()); };
130  const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); }
131 
133  size_t dim() const override { return Ab_.rows(); };
134 
136  double error(const Values& c) const override;
137 
143  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
144 
146  Vector error_vector(const Values& c) const;
147 
148 private:
150  friend class boost::serialization::access;
151  template<class ARCHIVE>
152  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
153  ar & boost::serialization::make_nvp("LinearizedJacobianFactor",
154  boost::serialization::base_object<Base>(*this));
155  ar & BOOST_SERIALIZATION_NVP(Ab_);
156  }
157 };
158 
160 template<>
161 struct traits<LinearizedJacobianFactor> : public Testable<LinearizedJacobianFactor> {
162 };
163 
168 class GTSAM_UNSTABLE_EXPORT LinearizedHessianFactor : public LinearizedGaussianFactor {
169 
170 public:
174 
176  typedef boost::shared_ptr<LinearizedHessianFactor> shared_ptr;
177 
179  typedef SymmetricBlockMatrix::Block Block;
180  typedef SymmetricBlockMatrix::constBlock constBlock;
181 
182  typedef SymmetricBlockMatrix::Block::ColXpr Column;
183  typedef SymmetricBlockMatrix::constBlock::ColXpr constColumn;
184 
185 protected:
186 
188 
190 public:
191 
194 
200  LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, const Values& lin_points);
201 
202  virtual ~LinearizedHessianFactor() {}
203 
205  gtsam::NonlinearFactor::shared_ptr clone() const override {
206  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
207  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
208 
209  // Testable
210 
212  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
213 
215  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
216 
220  double constantTerm() const {
221  const auto block = info_.diagonalBlock(size());
222  return block(0, 0);
223  }
224 
230  return info_.aboveDiagonalRange(j - begin(), size(), size(), size() + 1).col(0);
231  }
232 
236  return info_.aboveDiagonalRange(0, size(), size(), size() + 1).col(0);
237  }
238 
250  const DenseIndex J1 = j1 - begin();
251  const DenseIndex J2 = j2 - begin();
252  return info_.block(J1, J2);
253  }
254 
259  Eigen::SelfAdjointView<constBlock, Eigen::Upper> squaredTerm() const {
260  return info_.selfadjointView(0, size());
261  }
262 
264  size_t dim() const override { return info_.rows() - 1; }
265 
267  double error(const Values& c) const override;
268 
274  boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
275 
276 private:
278  friend class boost::serialization::access;
279  template<class ARCHIVE>
280  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
281  ar & boost::serialization::make_nvp("LinearizedHessianFactor",
282  boost::serialization::base_object<Base>(*this));
283  ar & BOOST_SERIALIZATION_NVP(info_);
284  }
285 };
286 
288 template<>
289 struct traits<LinearizedHessianFactor> : public Testable<LinearizedHessianFactor> {
290 };
291 
292 } // \namespace aspn
gtsam::LinearizedJacobianFactor
A factor that takes a linear, Jacobian factor and inserts it into a nonlinear graph.
Definition: LinearizedFactor.h:77
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
JacobianFactor.h
gtsam::LinearizedHessianFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: LinearizedFactor.h:205
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition: HessianFactor.h:110
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:175
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: VerticalBlockMatrix.h:114
gtsam::LinearizedGaussianFactor
A base factor class for the Jacobian and Hessian linearized factors.
Definition: LinearizedFactor.h:31
gtsam::LinearizedHessianFactor::info_
SymmetricBlockMatrix info_
The block view of the full information matrix, s.t.
Definition: LinearizedFactor.h:187
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::SymmetricBlockMatrix::block
Matrix block(DenseIndex I, DenseIndex J) const
Get a copy of a block (anywhere in the matrix).
Definition: SymmetricBlockMatrix.cpp:54
gtsam::LinearizedHessianFactor::constBlock
SymmetricBlockMatrix::constBlock constBlock
A block from the Hessian matrix (const version)
Definition: LinearizedFactor.h:180
gtsam::LinearizedJacobianFactor::shared_ptr
boost::shared_ptr< LinearizedJacobianFactor > shared_ptr
shared pointer for convenience
Definition: LinearizedFactor.h:85
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::LinearizedHessianFactor::linearTerm
constColumn linearTerm(const_iterator j) const
Return the part of linear term as described above corresponding to the requested variable.
Definition: LinearizedFactor.h:229
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition: VerticalBlockMatrix.h:129
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
gtsam::LinearizedJacobianFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: LinearizedFactor.h:115
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::LinearizedGaussianFactor::lin_points_
Values lin_points_
linearization points for error calculation
Definition: LinearizedFactor.h:43
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::LinearizedHessianFactor::squaredTerm
Matrix squaredTerm(const_iterator j1, const_iterator j2) const
Return a copy of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the squared term ,...
Definition: LinearizedFactor.h:249
gtsam::LinearizedHessianFactor::Block
SymmetricBlockMatrix::Block Block
hessian block data types
Definition: LinearizedFactor.h:179
gtsam::LinearizedHessianFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: LinearizedFactor.h:264
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::LinearizedGaussianFactor::shared_ptr
boost::shared_ptr< LinearizedGaussianFactor > shared_ptr
shared pointer for convenience
Definition: LinearizedFactor.h:38
gtsam::LinearizedGaussianFactor::LinearizedGaussianFactor
LinearizedGaussianFactor()
default constructor for serialization
Definition: LinearizedFactor.h:48
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::LinearizedHessianFactor::Column
SymmetricBlockMatrix::Block::ColXpr Column
A column containing the linear term h.
Definition: LinearizedFactor.h:182
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition: SymmetricBlockMatrix.h:119
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::LinearizedHessianFactor
A factor that takes a linear, Hessian factor and inserts it into a nonlinear graph.
Definition: LinearizedFactor.h:168
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:161
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
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:140
gtsam::LinearizedJacobianFactor::Base
LinearizedGaussianFactor Base
base type
Definition: LinearizedFactor.h:81
gtsam::LinearizedHessianFactor::constantTerm
double constantTerm() const
Return the constant term as described above.
Definition: LinearizedFactor.h:220
gtsam::SymmetricBlockMatrix
Definition: SymmetricBlockMatrix.h:52
gtsam::LinearizedHessianFactor::constColumn
SymmetricBlockMatrix::constBlock::ColXpr constColumn
A column containing the linear term h (const version)
Definition: LinearizedFactor.h:183
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::LinearizedHessianFactor::shared_ptr
boost::shared_ptr< LinearizedHessianFactor > shared_ptr
shared pointer for convenience
Definition: LinearizedFactor.h:176
gtsam::LinearizedHessianFactor::squaredTerm
Eigen::SelfAdjointView< constBlock, Eigen::Upper > squaredTerm() const
Return the <emph>upper-triangular part</emph> of the full squared term, as described above.
Definition: LinearizedFactor.h:259
gtsam::LinearizedJacobianFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: LinearizedFactor.h:133
gtsam::LinearizedHessianFactor::linearTerm
constColumn linearTerm() const
Return the complete linear term as described above.
Definition: LinearizedFactor.h:235
gtsam::LinearizedHessianFactor::Base
LinearizedGaussianFactor Base
base type
Definition: LinearizedFactor.h:172
gtsam::VerticalBlockMatrix
Definition: VerticalBlockMatrix.h:42
gtsam::LinearizedGaussianFactor::Base
NonlinearFactor Base
base type
Definition: LinearizedFactor.h:34