Main MRPT website > C++ reference for MRPT 1.9.9
ScalarFactorGraph.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/types_math.h>
14 #include <deque>
15 
16 namespace mrpt
17 {
18 namespace graphs
19 {
20 /** Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.
21  * The design of this class is optimized for large problems (e.g. >1e3 nodes,
22  * >1e4 constrainst)
23  * by leaving to the user/caller the responsibility of allocating all "nodes"
24  * and constraints.
25  * This class can be seen as an intermediary solution between current methods
26  * in mrpt::graphslam and the well-known G2O library:
27  *
28  * Assumptions/limitations:
29  * - Linear error functions (for now).
30  * - Scalar (1-dim) error functions.
31  * - Gaussian factors.
32  * - Solver: Eigen SparseQR.
33  *
34  * Usage:
35  * - Call initialize() to set the number of nodes.
36  * - Call addConstraints() to insert constraints. This may be called more than
37  * once.
38  * - Call updateEstimation() to run one step of the linear SparseQR solver.
39  *
40  * \ingroup mrpt_graph_grp
41  * \note [New in MRPT 1.5.0] Requires Eigen>=3.1
42  */
44 {
45  public:
47 
48  struct FactorBase
49  {
50  virtual ~FactorBase();
51  /** Return the residual/error of this observation. */
52  virtual double evaluateResidual() const = 0;
53  /** Return the inverse of the variance of this constraint */
54  virtual double getInformation() const = 0;
55  };
56 
57  /** Simple, scalar (1-dim) constraint (edge) for a GMRF */
59  {
60  size_t node_id;
61  /** Returns the derivative of the residual wrt the node value */
62  virtual void evalJacobian(double& dr_dx) const = 0;
63  };
64 
65  /** Simple, scalar (1-dim) constraint (edge) for a GMRF */
67  {
69  /** Returns the derivative of the residual wrt the node values */
70  virtual void evalJacobian(double& dr_dxi, double& dr_dxj) const = 0;
71  };
72 
73  /** Reset state: remove all constraints and nodes. */
74  void clear();
75 
76  /** Initialize the GMRF internal state and copy the prior factors. */
77  void initialize(
78  /** Number of unknown nodes in the MRF graph */
79  const size_t nodeCount);
80 
81  /** Insert constraints into the GMRF problem.
82  * \param listOfConstraints List of user-implemented constraints.
83  * **A pointer to the passed object is kept**, but memory ownship *REMAINS*
84  * being responsability of the caller. This is
85  * done such that arrays/vectors of constraints can be more efficiently
86  * allocated if their type is known at build time.
87  */
88  void addConstraint(const UnaryFactorVirtualBase& listOfConstraints);
89  void addConstraint(const BinaryFactorVirtualBase& listOfConstraints);
90 
91  /** Removes a constraint. Return true if found and deleted correctly. */
92  bool eraseConstraint(const FactorBase& c);
93 
96  void updateEstimation(
97  /** Output increment of the current estimate. Caller must add this
98  vector to current state vector to obtain the optimal estimation. */
99  Eigen::VectorXd& solved_x_inc,
100  /** If !=nullptr, the variances of each estimate will be stored here. */
101  Eigen::VectorXd* solved_variances = nullptr);
102 
103  bool isProfilerEnabled() const { return m_enable_profiler; }
104  void enableProfiler(bool enable = true) { m_enable_profiler = enable; }
105  private:
106  /** number of nodes in the graph */
107  size_t m_numNodes;
108 
109  std::deque<const UnaryFactorVirtualBase*> m_factors_unary;
110  std::deque<const BinaryFactorVirtualBase*> m_factors_binary;
111 
114 
115 }; // End of class def.
116 
117 } // End of namespace
118 } // End of namespace
void initialize(const size_t nodeCount)
Initialize the GMRF internal state and copy the prior factors.
Simple, scalar (1-dim) constraint (edge) for a GMRF.
bool eraseConstraint(const FactorBase &c)
Removes a constraint.
std::deque< const BinaryFactorVirtualBase * > m_factors_binary
void enableProfiler(bool enable=true)
mrpt::system::CTimeLogger m_timelogger
std::deque< const UnaryFactorVirtualBase * > m_factors_unary
const GLubyte * c
Definition: glext.h:6313
Versatile class for consistent logging and management of output messages.
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
virtual double evaluateResidual() const =0
Return the residual/error of this observation.
size_t m_numNodes
number of nodes in the graph
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
virtual void evalJacobian(double &dr_dx) const =0
Returns the derivative of the residual wrt the node value.
void clear()
Reset state: remove all constraints and nodes.
Simple, scalar (1-dim) constraint (edge) for a GMRF.
virtual double getInformation() const =0
Return the inverse of the variance of this constraint.
virtual void evalJacobian(double &dr_dxi, double &dr_dxj) const =0
Returns the derivative of the residual wrt the node values.
void updateEstimation(Eigen::VectorXd &solved_x_inc, Eigen::VectorXd *solved_variances=nullptr)
Sparse solver for GMRF (Gaussian Markov Random Fields) graphical models.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019