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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019