MRPT  1.9.9
ScalarFactorGraph.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "graphs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/system/CTicTac.h>
14 #include <Eigen/Dense>
15 
16 using namespace mrpt;
17 using namespace mrpt::graphs;
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 
28 {
29  MRPT_LOG_DEBUG("clear() called");
30 
31  m_numNodes = 0;
32  m_factors_unary.clear();
33  m_factors_binary.clear();
34 }
35 
36 void ScalarFactorGraph::initialize(const size_t nodeCount)
37 {
38  MRPT_LOG_DEBUG_STREAM("initialize() called, nodeCount=" << nodeCount);
39 
40  m_numNodes = nodeCount;
41 }
42 
44 {
45  m_factors_unary.push_back(&c);
46 }
48 {
49  m_factors_binary.push_back(&c);
50 }
51 
53 {
54  {
55  auto it = std::find(m_factors_unary.begin(), m_factors_unary.end(), &c);
56  if (it != m_factors_unary.end())
57  {
58  m_factors_unary.erase(it);
59  return true;
60  }
61  }
62  {
63  auto it =
64  std::find(m_factors_binary.begin(), m_factors_binary.end(), &c);
65  if (it != m_factors_binary.end())
66  {
67  m_factors_binary.erase(it);
68  return true;
69  }
70  }
71  return false;
72 }
73 
74 /* Method:
75  (\Sigma)^{-1/2) * d( h(x) )/d( x ) * x_incr = - (\Sigma)^{-1/2) * r(x)
76  =================================== ========================
77  =A =b
78 
79  A * x_incr = b --> SparseQR.
80 */
82  /** Output increment of the current estimate. Caller must add this
83  vector to current state vector to obtain the optimal estimation. */
84  mrpt::math::CVectorDouble& solved_x_inc,
85  /** If !=nullptr, the covariance of the estimate will be stored here. */
86  mrpt::math::CVectorDouble* solved_variances)
87 {
88  ASSERTMSG_(m_numNodes > 0, "numNodes=0. Have you called initialize()?");
89 
91 
92 #if EIGEN_VERSION_AT_LEAST(3, 1, 0)
93 
94  // Number of vertices:
95  const size_t n = m_numNodes;
96  solved_x_inc.setZero(n, 1);
97 
98  // Number of edges:
99  const size_t m1 = m_factors_unary.size(), m2 = m_factors_binary.size();
100  const size_t m = m1 + m2;
101 
102  // Build Ax=b
103  // -----------------------
104  m_timelogger.enter("GMRF.build_A_tri");
105 
106  std::vector<Eigen::Triplet<double>> A_tri;
107  A_tri.reserve(m1 + 2 * m2);
108 
109  Eigen::VectorXd g; // Error vector
110 
111  g.setZero(m);
112  int edge_counter = 0;
113  for (const auto& e : m_factors_unary)
114  {
115  ASSERT_(e != nullptr);
116  // Jacob:
117  const double w = std::sqrt(e->getInformation());
118  double dr_dx;
119  e->evalJacobian(dr_dx);
120  const int node_id = e->node_id;
121  A_tri.emplace_back(edge_counter, node_id, w * dr_dx);
122  // gradient:
123  g[edge_counter] -= w * e->evaluateResidual();
124 
125  ++edge_counter;
126  }
127  for (const auto& e : m_factors_binary)
128  {
129  ASSERT_(e != nullptr);
130  // Jacob:
131  const double w = std::sqrt(e->getInformation());
132  double dr_dxi, dr_dxj;
133  e->evalJacobian(dr_dxi, dr_dxj);
134  const int node_id_i = e->node_id_i, node_id_j = e->node_id_j;
135  A_tri.emplace_back(edge_counter, node_id_i, w * dr_dxi);
136  A_tri.emplace_back(edge_counter, node_id_j, w * dr_dxj);
137  // gradient:
138  g[edge_counter] -= w * e->evaluateResidual();
139 
140  ++edge_counter;
141  }
142  m_timelogger.leave("GMRF.build_A_tri");
143 
144  // Compress sparse
145  // -----------------------
146  Eigen::SparseMatrix<double> A(m, n);
147  {
149  m_timelogger, "GMRF.build_A_compress");
150 
151  A.setFromTriplets(A_tri.begin(), A_tri.end());
152  A.makeCompressed();
153  }
154 
155  // Solve increment
156  // -----------------------
157  Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
158  solver;
159  {
160  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "GMRF.solve");
161 
162  solver.compute(A);
163  solved_x_inc = solver.solve(g);
164  }
165 
166  // Recover covariance
167  // -----------------------
168  if (solved_variances)
169  {
170  mrpt::system::CTimeLoggerEntry tle(m_timelogger, "GMRF.variance");
171 
172  solved_variances->resize(n);
173 
174  // VARIANCE SIGMA = inv(P) * inv( P*H*inv(P) ) * P
175  // Get triangular supperior P*H*inv(P) = UT' * UT = P * R'*R * inv(P)
176  // (QR factor: Use UT=R)
177 
178  MRPT_TODO("Use compressed access instead of coeff() below");
179 
180  Eigen::SparseMatrix<double> UT = solver.matrixR();
181  Eigen::SparseMatrix<double> solved_covariance(n, n);
182  solved_covariance.reserve(UT.nonZeros());
183 
184  // Apply custom equations to obtain the inverse -> inv( P*H*inv(P) )
185  const int show_progress_steps = std::max(int(20), int(n / 20));
186  for (int l = n - 1; l >= 0; l--)
187  {
188  if (!(l % show_progress_steps))
190  "Computing variance %6.02f%%... \r",
191  (100.0 * (n - l - 1)) / n);
192 
193  // Computes variances in the inferior submatrix of "l"
194  double subSigmas = 0.0;
195  for (size_t j = l + 1; j < n; j++)
196  {
197  if (UT.coeff(l, j) != 0)
198  {
199  // Compute off-diagonal variances Sigma(j,l) = Sigma(l,j);
200 
201  // SUM 1
202  double sum = 0.0;
203  for (size_t i = l + 1; i <= j; i++)
204  {
205  if (UT.coeff(l, i) != 0)
206  {
207  sum +=
208  UT.coeff(l, i) * solved_covariance.coeff(i, j);
209  }
210  }
211  // SUM 2
212  for (size_t i = j + 1; i < n; ++i)
213  {
214  if (UT.coeff(l, i) != 0)
215  {
216  sum +=
217  UT.coeff(l, i) * solved_covariance.coeff(j, i);
218  }
219  }
220  // Save off-diagonal variance (only Upper triangular)
221  solved_covariance.insert(l, j) = (-sum / UT.coeff(l, l));
222  subSigmas += UT.coeff(l, j) * solved_covariance.coeff(l, j);
223  }
224  }
225 
226  solved_covariance.insert(l, l) =
227  (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.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
void updateEstimation(mrpt::math::CVectorDouble &solved_x_inc, mrpt::math::CVectorDouble *solved_variances=nullptr)
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:224
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
std::deque< const BinaryFactorVirtualBase * > m_factors_binary
STL namespace.
mrpt::system::COutputLogger COutputLogger
mrpt::system::CTimeLogger m_timelogger
std::deque< const UnaryFactorVirtualBase * > m_factors_unary
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
#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)
void enter(const std::string_view &func_name) noexcept
Start of a named section.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double leave(const std::string_view &func_name) noexcept
End of a named section.
void clear()
Reset state: remove all constraints and nodes.
MRPT_TODO("toPointCloud / calibration")
Simple, scalar (1-dim) constraint (edge) for a GMRF.
void resize(std::size_t N, bool zeroNewElements=false)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020