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
12/**
13 * @file LinearizedFactor.h
14 * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
15 * @author Alex Cunningham
16 */
17
18#pragma once
19
20#include <vector>
21#include <gtsam_unstable/dllexport.h>
22#include <gtsam/nonlinear/NonlinearFactor.h>
23#include <gtsam/linear/JacobianFactor.h>
24#include <gtsam/linear/HessianFactor.h>
25
26namespace gtsam {
27
28/**
29 * A base factor class for the Jacobian and Hessian linearized factors
30 */
31class GTSAM_UNSTABLE_EXPORT LinearizedGaussianFactor : public NonlinearFactor {
32public:
33 /** base type */
34 typedef NonlinearFactor Base;
35 typedef LinearizedGaussianFactor This;
36
37 /** shared pointer for convenience */
38 typedef std::shared_ptr<LinearizedGaussianFactor> shared_ptr;
39
40protected:
41
42 /** linearization points for error calculation */
43 Values lin_points_;
44
45public:
46
47 /** default constructor for serialization */
48 LinearizedGaussianFactor() = default;
49
50 /**
51 * @param gaussian: A jacobian or hessian factor
52 * @param lin_points: The linearization points for, at least, the variables used by this factor
53 */
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
61private:
62#if GTSAM_ENABLE_BOOST_SERIALIZATION
63 /** Serialization function */
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(n: "LinearizedGaussianFactor",
68 v&: boost::serialization::base_object<Base>(d&: *this));
69 ar & BOOST_SERIALIZATION_NVP(lin_points_);
70 }
71#endif
72
73};
74
75/**
76 * A factor that takes a linear, Jacobian factor and inserts it into
77 * a nonlinear graph.
78 */
79class GTSAM_UNSTABLE_EXPORT LinearizedJacobianFactor : public LinearizedGaussianFactor {
80
81public:
82 /** base type */
83 typedef LinearizedGaussianFactor Base;
84 typedef LinearizedJacobianFactor This;
85
86 /** shared pointer for convenience */
87 typedef std::shared_ptr<LinearizedJacobianFactor> shared_ptr;
88
89 typedef VerticalBlockMatrix::Block ABlock;
90 typedef VerticalBlockMatrix::constBlock constABlock;
91 typedef VerticalBlockMatrix::Block::ColXpr BVector;
92 typedef VerticalBlockMatrix::constBlock::ConstColXpr constBVector;
93
94protected:
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
103public:
104
105 /** default constructor for serialization */
106 LinearizedJacobianFactor();
107
108 /**
109 * @param jacobian: A jacobian factor
110 * @param lin_points: The linearization points for, at least, the variables used by this factor
111 */
112 LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, const Values& lin_points);
113
114 ~LinearizedJacobianFactor() override {}
115
116 /// @return a deep copy of this factor
117 gtsam::NonlinearFactor::shared_ptr clone() const override {
118 return std::static_pointer_cast<gtsam::NonlinearFactor>(
119 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
120
121 // Testable
122
123 /** print function */
124 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
125
126 /** equals function with optional tolerance */
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(i: 0); }
131 const constABlock A() const { return Ab_.range(startBlock: 0, endBlock: size()); }
132 const constABlock A(Key key) const { return Ab_(std::find(first: begin(), last: end(), val: key) - begin()); }
133
134 /** get the dimension of the factor (number of rows on linearization) */
135 size_t dim() const override { return Ab_.rows(); }
136
137 /** Calculate the error of the factor */
138 double error(const Values& c) const override;
139
140 /**
141 * linearize to a GaussianFactor
142 * Reimplemented from NoiseModelFactor to directly copy out the
143 * matrices and only update the RHS b with an updated residual
144 */
145 std::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
146
147 /** (A*x-b) */
148 Vector error_vector(const Values& c) const;
149
150private:
151#if GTSAM_ENABLE_BOOST_SERIALIZATION
152 friend class boost::serialization::access;
153 /** Serialization function */
154 template<class ARCHIVE>
155 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
156 ar & boost::serialization::make_nvp(n: "LinearizedJacobianFactor",
157 v&: boost::serialization::base_object<Base>(d&: *this));
158 ar & BOOST_SERIALIZATION_NVP(Ab_);
159 }
160#endif
161};
162
163/// traits
164template<>
165struct traits<LinearizedJacobianFactor> : public Testable<LinearizedJacobianFactor> {
166};
167
168/**
169 * A factor that takes a linear, Hessian factor and inserts it into
170 * a nonlinear graph.
171 */
172class GTSAM_UNSTABLE_EXPORT LinearizedHessianFactor : public LinearizedGaussianFactor {
173
174public:
175 /** base type */
176 typedef LinearizedGaussianFactor Base;
177 typedef LinearizedHessianFactor This;
178
179 /** shared pointer for convenience */
180 typedef std::shared_ptr<LinearizedHessianFactor> shared_ptr;
181
182 /** hessian block data types */
183 typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
184 typedef SymmetricBlockMatrix::constBlock constBlock; ///< A block from the Hessian matrix (const version)
185
186 typedef SymmetricBlockMatrix::Block::ColXpr Column; ///< A column containing the linear term h
187 typedef SymmetricBlockMatrix::constBlock::ColXpr constColumn; ///< A column containing the linear term h (const version)
188
189protected:
190
191 SymmetricBlockMatrix info_; ///< The block view of the full information matrix, s.t. the quadratic
192 /// error is 0.5*[x -1]'*H*[x -1]
193
194public:
195
196 /** default constructor for serialization */
197 LinearizedHessianFactor();
198
199 /**
200 * Use this constructor with the ordering used to linearize the graph
201 * @param hessian: A hessian factor
202 * @param lin_points: The linearization points for, at least, the variables used by this factor
203 */
204 LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, const Values& lin_points);
205
206 ~LinearizedHessianFactor() override {}
207
208 /// @return a deep copy of this factor
209 gtsam::NonlinearFactor::shared_ptr clone() const override {
210 return std::static_pointer_cast<gtsam::NonlinearFactor>(
211 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
212
213 // Testable
214
215 /** print function */
216 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
217
218 /** equals function with optional tolerance */
219 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
220
221 /** Return the constant term \f$ f \f$ as described above
222 * @return The constant term \f$ f \f$
223 */
224 double constantTerm() const {
225 const auto block = info_.diagonalBlock(J: size());
226 return block(0, 0);
227 }
228
229 /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
230 * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can
231 * use, for example, begin() + 2 to get the 3rd variable in this factor.
232 * @return The linear term \f$ g \f$ */
233 constColumn linearTerm(const_iterator j) const {
234 return info_.aboveDiagonalRange(i_startBlock: j - begin(), i_endBlock: size(), j_startBlock: size(), j_endBlock: size() + 1).col(i: 0);
235 }
236
237 /** Return the complete linear term \f$ g \f$ as described above.
238 * @return The linear term \f$ g \f$ */
239 constColumn linearTerm() const {
240 return info_.aboveDiagonalRange(i_startBlock: 0, i_endBlock: size(), j_startBlock: size(), j_endBlock: size() + 1).col(i: 0);
241 }
242
243 /** Return a copy of the block at (j1,j2) of the <em>upper-triangular part</em> of the
244 * squared term \f$ H \f$, no data is copied. See HessianFactor class documentation
245 * above to explain that only the upper-triangular part of the information matrix is stored
246 * and returned by this function.
247 * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can
248 * use, for example, begin() + 2 to get the 3rd variable in this factor.
249 * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can
250 * use, for example, begin() + 2 to get the 3rd variable in this factor.
251 * @return A copy of the requested block.
252 */
253 Matrix squaredTerm(const_iterator j1, const_iterator j2) const {
254 const DenseIndex J1 = j1 - begin();
255 const DenseIndex J2 = j2 - begin();
256 return info_.block(I: J1, J: J2);
257 }
258
259 /** Return the <em>upper-triangular part</em> of the full squared term, as described above.
260 * See HessianFactor class documentation above to explain that only the
261 * upper-triangular part of the information matrix is stored and returned by this function.
262 */
263 Eigen::SelfAdjointView<constBlock, Eigen::Upper> squaredTerm() const {
264 return info_.selfadjointView(I: 0, J: size());
265 }
266
267 /** get the dimension of the factor (number of rows on linearization) */
268 size_t dim() const override { return info_.rows() - 1; }
269
270 /** Calculate the error of the factor */
271 double error(const Values& c) const override;
272
273 /**
274 * linearize to a GaussianFactor
275 * Reimplemented from NoiseModelFactor to directly copy out the
276 * matrices and only update the RHS b with an updated residual
277 */
278 std::shared_ptr<GaussianFactor> linearize(const Values& c) const override;
279
280private:
281 /** Serialization function */
282#if 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(n: "LinearizedHessianFactor",
287 v&: boost::serialization::base_object<Base>(d&: *this));
288 ar & BOOST_SERIALIZATION_NVP(info_);
289 }
290#endif
291};
292
293/// traits
294template<>
295struct traits<LinearizedHessianFactor> : public Testable<LinearizedHessianFactor> {
296};
297
298} // \namespace aspn
299