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 GaussianFactorGraph.h
14 * @brief Linear Factor Graph where all factors are Gaussians
15 * @author Kai Ni
16 * @author Christian Potthast
17 * @author Alireza Fathi
18 * @author Richard Roberts
19 * @author Frank Dellaert
20 */
21
22#pragma once
23
24#include <cstddef>
25#include <gtsam/inference/EliminateableFactorGraph.h>
26#include <gtsam/inference/FactorGraph.h>
27#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
28#include <gtsam/linear/GaussianFactor.h>
29#include <gtsam/linear/HessianFactor.h>
30#include <gtsam/linear/JacobianFactor.h>
31#include <gtsam/linear/VectorValues.h>
32
33namespace gtsam {
34
35 // Forward declarations
36 class GaussianFactorGraph;
37 class GaussianFactor;
38 class GaussianConditional;
39 class GaussianBayesNet;
40 class GaussianEliminationTree;
41 class GaussianBayesTree;
42 class GaussianJunctionTree;
43
44 /* ************************************************************************* */
45 template<> struct EliminationTraits<GaussianFactorGraph>
46 {
47 typedef GaussianFactor FactorType; ///< Type of factors in factor graph
48 typedef GaussianFactorGraph FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph)
49 typedef GaussianConditional ConditionalType; ///< Type of conditionals from elimination
50 typedef GaussianBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination
51 typedef GaussianEliminationTree EliminationTreeType; ///< Type of elimination tree
52 typedef GaussianBayesTree BayesTreeType; ///< Type of Bayes tree
53 typedef GaussianJunctionTree JunctionTreeType; ///< Type of Junction tree
54 /// The default dense elimination function
55 static std::pair<std::shared_ptr<ConditionalType>, std::shared_ptr<FactorType> >
56 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
57 return EliminatePreferCholesky(factors, keys); }
58 /// The default ordering generation function
59 static Ordering DefaultOrderingFunc(
60 const FactorGraphType& graph,
61 std::optional<std::reference_wrapper<const VariableIndex>> variableIndex) {
62 return Ordering::Colamd(variableIndex: (*variableIndex).get());
63 }
64 };
65
66 /* ************************************************************************* */
67 /**
68 * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
69 * Factor == GaussianFactor
70 * VectorValues = A values structure of vectors
71 * Most of the time, linear factor graphs arise by linearizing a non-linear factor graph.
72 */
73 class GTSAM_EXPORT GaussianFactorGraph :
74 public FactorGraph<GaussianFactor>,
75 public EliminateableFactorGraph<GaussianFactorGraph>
76 {
77 public:
78
79 typedef GaussianFactorGraph This; ///< Typedef to this class
80 typedef FactorGraph<GaussianFactor> Base; ///< Typedef to base factor graph type
81 typedef EliminateableFactorGraph<This> BaseEliminateable; ///< Typedef to base elimination class
82 typedef std::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
83
84 /// @name Constructors
85 /// @{
86
87 /** Default constructor */
88 GaussianFactorGraph() {}
89
90 /**
91 * Construct from an initializer lists of GaussianFactor shared pointers.
92 * Example:
93 * GaussianFactorGraph graph = { factor1, factor2, factor3 };
94 */
95 GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
96
97
98 /** Construct from iterator over factors */
99 template<typename ITERATOR>
100 GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
101
102 /** Construct from container of factors (shared_ptr or plain objects) */
103 template<class CONTAINER>
104 explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}
105
106 /** Implicit copy/downcast constructor to override explicit template container constructor */
107 template<class DERIVEDFACTOR>
108 GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
109
110 /// @}
111 /// @name Testable
112 /// @{
113
114 bool equals(const This& fg, double tol = 1e-9) const;
115
116 /// @}
117
118 /// Check exact equality.
119 friend bool operator==(const GaussianFactorGraph& lhs,
120 const GaussianFactorGraph& rhs) {
121 return lhs.isEqual(other: rhs);
122 }
123
124 /** Add a factor by value - makes a copy */
125 void add(const GaussianFactor& factor) { push_back(factor: factor.clone()); }
126
127 /** Add a factor by pointer - stores pointer without copying the factor */
128 void add(const sharedFactor& factor) { push_back(factor); }
129
130 /** Add a null factor */
131 void add(const Vector& b) {
132 add(factor: JacobianFactor(b)); }
133
134 /** Add a unary factor */
135 void add(Key key1, const Matrix& A1,
136 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
137 add(factor: JacobianFactor(key1,A1,b,model)); }
138
139 /** Add a binary factor */
140 void add(Key key1, const Matrix& A1,
141 Key key2, const Matrix& A2,
142 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
143 add(factor: JacobianFactor(key1,A1,key2,A2,b,model)); }
144
145 /** Add a ternary factor */
146 void add(Key key1, const Matrix& A1,
147 Key key2, const Matrix& A2,
148 Key key3, const Matrix& A3,
149 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
150 add(factor: JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); }
151
152 /** Add an n-ary factor */
153 template<class TERMS>
154 void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
155 add(factor: JacobianFactor(terms,b,model)); }
156
157 /**
158 * Return the set of variables involved in the factors (computes a set
159 * union).
160 */
161 typedef KeySet Keys;
162 Keys keys() const;
163
164 /* return a map of (Key, dimension) */
165 std::map<Key, size_t> getKeyDimMap() const;
166
167 /** unnormalized error */
168 double error(const VectorValues& x) const;
169
170 /** Unnormalized probability. O(n) */
171 double probPrime(const VectorValues& c) const;
172
173 /**
174 * Clone() performs a deep-copy of the graph, including all of the factors.
175 * Cloning preserves null factors so indices for the original graph are still
176 * valid for the cloned graph.
177 */
178 virtual GaussianFactorGraph clone() const;
179
180 /**
181 * CloneToPtr() performs a simple assignment to a new graph and returns it.
182 * There is no preservation of null factors!
183 */
184 virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
185
186 /**
187 * Returns the negation of all factors in this graph - corresponds to antifactors.
188 * Will convert all factors to HessianFactors due to negation of information.
189 * Cloning preserves null factors so indices for the original graph are still
190 * valid for the cloned graph.
191 */
192 GaussianFactorGraph negate() const;
193
194 ///@name Linear Algebra
195 ///@{
196
197 /**
198 * Returns a sparse augmented Jacbian matrix as a vector of i, j, and s,
199 * where i(k) and j(k) are the base 0 row and column indices, and s(k) is
200 * the entry as a double.
201 * The standard deviations are baked into A and b
202 * @return the sparse matrix as a std::vector of std::tuples
203 * @param ordering the column ordering
204 * @param[out] nrows The number of rows in the augmented Jacobian
205 * @param[out] ncols The number of columns in the augmented Jacobian
206 */
207 std::vector<std::tuple<int, int, double> > sparseJacobian(
208 const Ordering& ordering, size_t& nrows, size_t& ncols) const;
209
210 /** Returns a sparse augmented Jacobian matrix with default ordering */
211 std::vector<std::tuple<int, int, double> > sparseJacobian() const;
212
213 /**
214 * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s]
215 * entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's
216 * sparse. Note: i, j are 1-indexed.
217 * The standard deviations are baked into A and b
218 */
219 Matrix sparseJacobian_() const;
220
221 /**
222 * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
223 * Jacobian matrix, augmented with b with the noise models baked
224 * into A and b. The negative log-likelihood is
225 * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
226 * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
227 */
228 Matrix augmentedJacobian(const Ordering& ordering) const;
229
230 /**
231 * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
232 * Jacobian matrix, augmented with b with the noise models baked
233 * into A and b. The negative log-likelihood is
234 * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
235 * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
236 */
237 Matrix augmentedJacobian() const;
238
239 /**
240 * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
241 * with the noise models baked into A and b. The negative log-likelihood
242 * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
243 * GaussianFactorGraph::augmentedJacobian and
244 * GaussianFactorGraph::sparseJacobian.
245 */
246 std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
247
248 /**
249 * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
250 * with the noise models baked into A and b. The negative log-likelihood
251 * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
252 * GaussianFactorGraph::augmentedJacobian and
253 * GaussianFactorGraph::sparseJacobian.
254 */
255 std::pair<Matrix,Vector> jacobian() const;
256
257 /**
258 * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
259 * matrix, augmented with the information vector \f$ \eta \f$. The
260 * augmented Hessian is
261 \f[ \left[ \begin{array}{ccc}
262 \Lambda & \eta \\
263 \eta^T & c
264 \end{array} \right] \f]
265 and the negative log-likelihood is
266 \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
267 */
268 Matrix augmentedHessian(const Ordering& ordering) const;
269
270 /**
271 * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
272 * matrix, augmented with the information vector \f$ \eta \f$. The
273 * augmented Hessian is
274 \f[ \left[ \begin{array}{ccc}
275 \Lambda & \eta \\
276 \eta^T & c
277 \end{array} \right] \f]
278 and the negative log-likelihood is
279 \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
280 */
281 Matrix augmentedHessian() const;
282
283 /**
284 * Return the dense Hessian \f$ \Lambda \f$ and information vector
285 * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
286 * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
287 * GaussianFactorGraph::augmentedHessian.
288 */
289 std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
290
291 /**
292 * Return the dense Hessian \f$ \Lambda \f$ and information vector
293 * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
294 * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
295 * GaussianFactorGraph::augmentedHessian.
296 */
297 std::pair<Matrix,Vector> hessian() const;
298
299 /** Return only the diagonal of the Hessian A'*A, as a VectorValues */
300 virtual VectorValues hessianDiagonal() const;
301
302 /** Return the block diagonal of the Hessian for this factor */
303 virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
304
305 /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
306 * the dense elimination function specified in \c function (default EliminatePreferCholesky),
307 * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent
308 * to calling graph.eliminateMultifrontal()->optimize(). */
309 VectorValues optimize(
310 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
311
312 /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
313 * the dense elimination function specified in \c function (default EliminatePreferCholesky),
314 * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent
315 * to calling graph.eliminateMultifrontal()->optimize(). */
316 VectorValues optimize(const Ordering& ordering,
317 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
318
319 /**
320 * Optimize using Eigen's dense Cholesky factorization
321 */
322 VectorValues optimizeDensely() const;
323
324 /**
325 * Compute the gradient of the energy function,
326 * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
327 * centered around \f$ x = x_0 \f$.
328 * The gradient is \f$ A^T(Ax-b) \f$.
329 * @param fg The Jacobian factor graph $(A,b)$
330 * @param x0 The center about which to compute the gradient
331 * @return The gradient as a VectorValues
332 */
333 VectorValues gradient(const VectorValues& x0) const;
334
335 /**
336 * Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b
337 * \right\Vert^2 \f$, centered around zero. The gradient is \f$ A^T(Ax-b) \f$.
338 * @param fg The Jacobian factor graph $(A,b)$
339 * @param [output] g A VectorValues to store the gradient, which must be preallocated,
340 * see allocateVectorValues
341 * @return The gradient as a VectorValues */
342 virtual VectorValues gradientAtZero() const;
343
344 /** Optimize along the gradient direction, with a closed-form computation to perform the line
345 * search. The gradient is computed about \f$ \delta x=0 \f$.
346 *
347 * This function returns \f$ \delta x \f$ that minimizes a reparametrized problem. The error
348 * function of a GaussianBayesNet is
349 *
350 * \f[ f(\delta x) = \frac{1}{2} |R \delta x - d|^2 = \frac{1}{2}d^T d - d^T R \delta x +
351 * \frac{1}{2} \delta x^T R^T R \delta x \f]
352 *
353 * with gradient and Hessian
354 *
355 * \f[ g(\delta x) = R^T(R\delta x - d), \qquad G(\delta x) = R^T R. \f]
356 *
357 * This function performs the line search in the direction of the gradient evaluated at \f$ g =
358 * g(\delta x = 0) \f$ with step size \f$ \alpha \f$ that minimizes \f$ f(\delta x = \alpha g)
359 * \f$:
360 *
361 * \f[ f(\alpha) = \frac{1}{2} d^T d + g^T \delta x + \frac{1}{2} \alpha^2 g^T G g \f]
362 *
363 * Optimizing by setting the derivative to zero yields \f$ \hat \alpha = (-g^T g) / (g^T G g)
364 * \f$. For efficiency, this function evaluates the denominator without computing the Hessian
365 * \f$ G \f$, returning
366 *
367 * \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f] */
368 VectorValues optimizeGradientSearch() const;
369
370 /** x = A'*e */
371 VectorValues transposeMultiply(const Errors& e) const;
372
373 /** x += alpha*A'*e */
374 void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
375
376 /** return A*x-b */
377 Errors gaussianErrors(const VectorValues& x) const;
378
379 ///** return A*x */
380 Errors operator*(const VectorValues& x) const;
381
382 ///** y += alpha*A'A*x */
383 void multiplyHessianAdd(double alpha, const VectorValues& x,
384 VectorValues& y) const;
385
386 ///** In-place version e <- A*x that overwrites e. */
387 void multiplyInPlace(const VectorValues& x, Errors& e) const;
388
389 /** In-place version e <- A*x that takes an iterator. */
390 void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
391
392 void printErrors(
393 const VectorValues& x,
394 const std::string& str = "GaussianFactorGraph: ",
395 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
396 const std::function<bool(const Factor* /*factor*/,
397 double /*whitenedError*/, size_t /*index*/)>&
398 printCondition =
399 [](const Factor*, double, size_t) { return true; }) const;
400 /// @}
401
402 private:
403#if GTSAM_ENABLE_BOOST_SERIALIZATION
404 /** Serialization function */
405 friend class boost::serialization::access;
406 template<class ARCHIVE>
407 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
408 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
409 }
410#endif
411 };
412
413 /**
414 * Evaluates whether linear factors have any constrained noise models
415 * @return true if any factor is constrained.
416 */
417 GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
418
419 /****** Linear Algebra Operations ******/
420
421 ///* matrix-vector operations */
422 //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
423 //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
424
425/// traits
426template<>
427struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
428};
429
430} // \ namespace gtsam
431