MRPT  1.9.9
ScalarFactorGraph.cpp
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 
10 #include "graphs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/system/CTicTac.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::graphs;
17 using namespace std;
18 
19 #if EIGEN_VERSION_AT_LEAST(3, 1, 0) // Requires Eigen>=3.1
20 #include <Eigen/SparseCore>
21 #include <Eigen/SparseQR>
22 #endif
23 
26  : COutputLogger("GMRF"), m_enable_profiler(false)
27 {
28 }
29 
31 {
32  MRPT_LOG_DEBUG("clear() called");
33 
34  m_numNodes = 0;
35  m_factors_unary.clear();
36  m_factors_binary.clear();
37 }
38 
39 void ScalarFactorGraph::initialize(const size_t nodeCount)
40 {
41  MRPT_LOG_DEBUG_STREAM("initialize() called, nodeCount=" << nodeCount);
42 
43  m_numNodes = nodeCount;
44 }
45 
47 {
48  m_factors_unary.push_back(&c);
49 }
51 {
52  m_factors_binary.push_back(&c);
53 }
54 
56 {
57  {
58  auto it = std::find(m_factors_unary.begin(), m_factors_unary.end(), &c);
59  if (it != m_factors_unary.end())
60  {
61  m_factors_unary.erase(it);
62  return true;
63  }
64  }
65  {
66  auto it =
67  std::find(m_factors_binary.begin(), m_factors_binary.end(), &c);
68  if (it != m_factors_binary.end())
69  {
70  m_factors_binary.erase(it);
71  return true;
72  }
73  }
74  return false;
75 }
76 
77 /* Method:
78  (\Sigma)^{-1/2) * d( h(x) )/d( x ) * x_incr = - (\Sigma)^{-1/2) * r(x)
79  =================================== ========================
80  =A =b
81 
82  A * x_incr = b --> SparseQR.
83 */
85  /** Output increment of the current estimate. Caller must add this
86  vector to current state vector to obtain the optimal estimation. */
87  Eigen::VectorXd& solved_x_inc,
88  /** If !=nullptr, the covariance of the estimate will be stored here. */
89  Eigen::VectorXd* solved_variances)
90 {
91  ASSERTMSG_(m_numNodes > 0, "numNodes=0. Have you called initialize()?");
92 
94 
95 #if EIGEN_VERSION_AT_LEAST(3, 1, 0)
96 
97  // Number of vertices:
98  const size_t n = m_numNodes;
99  solved_x_inc.setZero(n);
100 
101  // Number of edges:
102  const size_t m1 = m_factors_unary.size(), m2 = m_factors_binary.size();
103  const size_t m = m1 + m2;
104 
105  // Build Ax=b
106  // -----------------------
107  m_timelogger.enter("GMRF.build_A_tri");
108 
109  std::vector<Eigen::Triplet<double>> A_tri;
110  A_tri.reserve(m1 + 2 * m2);
111 
112  Eigen::VectorXd g; // Error vector
113 
114  g.setZero(m);
115  int edge_counter = 0;
116  for (const auto& e : m_factors_unary)
117  {
118  ASSERT_(e != nullptr);
119  // Jacob:
120  const double w = std::sqrt(e->getInformation());
121  double dr_dx;
122  e->evalJacobian(dr_dx);
123  const int node_id = e->node_id;
124  A_tri.push_back(
125  Eigen::Triplet<double>(edge_counter, node_id, w * dr_dx));
126  // gradient:
127  g[edge_counter] -= w * e->evaluateResidual();
128 
129  ++edge_counter;
130  }
131  for (const auto& e : m_factors_binary)
132  {
133  ASSERT_(e != nullptr);
134  // Jacob:
135  const double w = std::sqrt(e->getInformation());
136  double dr_dxi, dr_dxj;
137  e->evalJacobian(dr_dxi, dr_dxj);
138  const int node_id_i = e->node_id_i, node_id_j = e->node_id_j;
139  A_tri.push_back(
140  Eigen::Triplet<double>(edge_counter, node_id_i, w * dr_dxi));
141  A_tri.push_back(
142  Eigen::Triplet<double>(edge_counter, node_id_j, w * dr_dxj));
143  // gradient:
144  g[edge_counter] -= w * e->evaluateResidual();
145 
146  ++edge_counter;
147  }
148  m_timelogger.leave("GMRF.build_A_tri");
149 
150  // Compress sparse
151  // -----------------------
152  Eigen::SparseMatrix<double> A(m, n);
153  {
155  m_timelogger, "GMRF.build_A_compress");
156 
157  A.setFromTriplets(A_tri.begin(), A_tri.end());
158  A.makeCompressed();
159  }
160 
161  // Solve increment
162  // -----------------------
163  Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
164  solver;
165  {
166  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "GMRF.solve");
167 
168  solver.compute(A);
169  solved_x_inc = solver.solve(g);
170  }
171 
172  // Recover covariance
173  // -----------------------
174  if (solved_variances)
175  {
176  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "GMRF.variance");
177 
178  solved_variances->resize(n);
179 
180  // VARIANCE SIGMA = inv(P) * inv( P*H*inv(P) ) * P
181  // Get triangular supperior P*H*inv(P) = UT' * UT = P * R'*R * inv(P)
182  // (QR factor: Use UT=R)
183 
184  MRPT_TODO("Use compressed access instead of coeff() below");
185 
186  Eigen::SparseMatrix<double> UT = solver.matrixR();
187  Eigen::SparseMatrix<double> solved_covariance(n, n);
188  solved_covariance.reserve(UT.nonZeros());
189 
190  // Apply custom equations to obtain the inverse -> inv( P*H*inv(P) )
191  const int show_progress_steps = std::max(int(20), int(n / 20));
192  for (int l = n - 1; l >= 0; l--)
193  {
194  if (!(l % show_progress_steps))
196  "Computing variance %6.02f%%... \r",
197  (100.0 * (n - l - 1)) / n);
198 
199  // Computes variances in the inferior submatrix of "l"
200  double subSigmas = 0.0;
201  for (size_t j = l + 1; j < n; j++)
202  {
203  if (UT.coeff(l, j) != 0)
204  {
205  // Compute off-diagonal variances Sigma(j,l) = Sigma(l,j);
206 
207  // SUM 1
208  double sum = 0.0;
209  for (size_t i = l + 1; i <= j; i++)
210  {
211  if (UT.coeff(l, i) != 0)
212  {
213  sum +=
214  UT.coeff(l, i) * solved_covariance.coeff(i, j);
215  }
216  }
217  // SUM 2
218  for (size_t i = j + 1; i < n; ++i)
219  {
220  if (UT.coeff(l, i) != 0)
221  {
222  sum +=
223  UT.coeff(l, i) * solved_covariance.coeff(j, i);
224  }
225  }
226  // Save off-diagonal variance (only Upper triangular)
227  solved_covariance.insert(l, j) = (-sum / UT.coeff(l, l));
228  subSigmas += UT.coeff(l, j) * solved_covariance.coeff(l, j);
229  }
230  }
231 
232  solved_covariance.insert(l, l) =
233  (1 / UT.coeff(l, l)) * (1 / UT.coeff(l, l) - subSigmas);
234  }
235 
236  MRPT_LOG_DEBUG_FMT("Computing variance %6.02f%%... \r", 100.0);
237 
238  for (unsigned int i = 0; i < n; i++)
239  {
240  const int idx = (int)solver.colsPermutation().indices().coeff(i);
241  const double variance = solved_covariance.coeff(i, i);
242  (*solved_variances)[idx] = variance;
243  }
244 
245  } // end calc variances
246 
247 #else
248  THROW_EXCEPTION("This method requires Eigen 3.1.0 or above");
249 #endif
250 }
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.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:217
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
A safe way to call enter() and leave() of a mrpt::system::CTimeLogger upon construction and destructi...
bool eraseConstraint(const FactorBase &c)
Removes a constraint.
Abstract graph and tree data structures, plus generic graph algorithms.
GLenum GLsizei n
Definition: glext.h:5074
std::deque< const BinaryFactorVirtualBase * > m_factors_binary
STL namespace.
mrpt::system::COutputLogger COutputLogger
mrpt::system::CTimeLogger m_timelogger
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
std::deque< const UnaryFactorVirtualBase * > m_factors_unary
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
const GLubyte * c
Definition: glext.h:6313
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
GLubyte g
Definition: glext.h:6279
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
double leave(const char *func_name)
End of a named section.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
size_t m_numNodes
number of nodes in the graph
void enable(bool enabled=true)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MRPT_TODO(x)
Definition: common.h:129
void clear()
Reset state: remove all constraints and nodes.
Simple, scalar (1-dim) constraint (edge) for a GMRF.
void updateEstimation(Eigen::VectorXd &solved_x_inc, Eigen::VectorXd *solved_variances=nullptr)
void enter(const char *func_name)
Start of a named section.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020