MRPT  2.0.5
levmarq_impl.h
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 #pragma once
10 
11 #include <Eigen/Dense>
12 #include <vector>
13 
14 namespace mrpt
15 {
16 namespace graphslam
17 {
18 /// Internal auxiliary classes
19 namespace detail
20 {
21 using namespace mrpt;
22 using namespace mrpt::poses;
23 using namespace mrpt::graphslam;
24 using namespace mrpt::math;
25 using namespace std;
26 
27 // An auxiliary struct to compute the pseudo-ln of a pose error, possibly
28 // modified with an information matrix.
29 // Specializations are below.
30 template <class EDGE, class gst>
31 struct AuxErrorEval;
32 
33 // For graphs of 2D constraints (no information matrix)
34 template <class gst>
35 struct AuxErrorEval<CPose2D, gst>
36 {
37  template <class MAT, class EDGE_ITERATOR>
38  static inline void multiplyJtLambdaJ(
39  const MAT& J1, MAT& JtJ, [[maybe_unused]] const EDGE_ITERATOR& edge)
40  {
41  JtJ.matProductOf_AtA(J1);
42  }
43 
44  template <class MAT, class EDGE_ITERATOR>
45  static inline void multiplyJ1tLambdaJ2(
46  const MAT& J1, const MAT& J2, MAT& JtJ,
47  [[maybe_unused]] const EDGE_ITERATOR& edge)
48  {
49  JtJ.asEigen() = J1.transpose() * J2.asEigen();
50  }
51 
52  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
53  static inline void multiply_Jt_W_err(
54  const JAC& J, [[maybe_unused]] const EDGE_ITERATOR& edge,
55  const VEC1& ERR, VEC2& OUT)
56  {
57  const auto grad_incr = (J.transpose() * ERR.asEigen()).eval();
58  OUT.asEigen() += grad_incr;
59  }
60 };
61 
62 // For graphs of 3D constraints (no information matrix)
63 template <class gst>
64 struct AuxErrorEval<CPose3D, gst>
65 {
66  template <class MAT, class EDGE_ITERATOR>
67  static inline void multiplyJtLambdaJ(
68  const MAT& J1, MAT& JtJ, [[maybe_unused]] const EDGE_ITERATOR& edge)
69  {
70  JtJ.matProductOf_AtA(J1);
71  }
72 
73  template <class MAT, class EDGE_ITERATOR>
74  static inline void multiplyJ1tLambdaJ2(
75  const MAT& J1, const MAT& J2, MAT& JtJ,
76  [[maybe_unused]] const EDGE_ITERATOR& edge)
77  {
78  JtJ.asEigen() = J1.transpose() * J2.asEigen();
79  }
80 
81  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
82  static inline void multiply_Jt_W_err(
83  const JAC& J, [[maybe_unused]] const EDGE_ITERATOR& edge,
84  const VEC1& ERR, VEC2& OUT)
85  {
86  OUT.asEigen() += J.transpose() * ERR.asEigen();
87  }
88 };
89 
90 // For graphs of 2D constraints (with information matrix)
91 template <class gst>
93 {
94  template <class MAT, class EDGE_ITERATOR>
95  static inline void multiplyJtLambdaJ(
96  const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge)
97  {
98  JtJ.asEigen() =
99  (J1.transpose() * edge->second.cov_inv.asEigen()) * J1.asEigen();
100  }
101  template <class MAT, class EDGE_ITERATOR>
102  static inline void multiplyJ1tLambdaJ2(
103  const MAT& J1, const MAT& J2, MAT& JtJ, const EDGE_ITERATOR& edge)
104  {
105  JtJ.asEigen() =
106  (J1.transpose() * edge->second.cov_inv.asEigen()) * J2.asEigen();
107  }
108 
109  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
110  static inline void multiply_Jt_W_err(
111  const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
112  {
113  OUT.asEigen() +=
114  (J.transpose() * edge->second.cov_inv.asEigen()) * ERR.asEigen();
115  }
116 };
117 
118 // For graphs of 3D constraints (with information matrix)
119 template <class gst>
121 {
122  template <class MAT, class EDGE_ITERATOR>
123  static inline void multiplyJtLambdaJ(
124  const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge)
125  {
126  JtJ.asEigen() =
127  (J1.transpose() * edge->second.cov_inv.asEigen()) * J1.asEigen();
128  }
129 
130  template <class MAT, class EDGE_ITERATOR>
131  static inline void multiplyJ1tLambdaJ2(
132  const MAT& J1, const MAT& J2, MAT& JtJ, const EDGE_ITERATOR& edge)
133  {
134  JtJ.asEigen() =
135  (J1.transpose() * edge->second.cov_inv.asEigen()) * J2.asEigen();
136  }
137 
138  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
139  static inline void multiply_Jt_W_err(
140  const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
141  {
142  OUT.asEigen() +=
143  (J.transpose() * edge->second.cov_inv.asEigen()) * ERR.asEigen();
144  }
145 };
146 
147 } // namespace detail
148 
149 // Compute, at once, jacobians and the error vectors for each constraint in
150 // "lstObservationData", returns the overall squared error.
151 template <class GRAPH_T>
153  [[maybe_unused]] const GRAPH_T& graph,
154  const std::vector<typename graphslam_traits<GRAPH_T>::observation_info_t>&
155  lstObservationData,
157  std::vector<typename graphslam_traits<GRAPH_T>::Array_O>& errs)
158 {
159  using gst = graphslam_traits<GRAPH_T>;
160 
161  lstJacobians.clear();
162  errs.clear();
163 
164  const size_t nObservations = lstObservationData.size();
165 
166  for (size_t i = 0; i < nObservations; i++)
167  {
168  const typename gst::observation_info_t& obs = lstObservationData[i];
169  auto e = obs.edge;
170  const typename gst::graph_t::constraint_t::type_value* EDGE_POSE =
171  obs.edge_mean;
172  typename gst::graph_t::constraint_t::type_value* P1 = obs.P1;
173  typename gst::graph_t::constraint_t::type_value* P2 = obs.P2;
174 
175  const auto& ids = e->first;
176  // const auto& edge = e->second;
177 
178  // Compute the residual pose error of these pair of nodes + its
179  // constraint:
180  // DinvP1invP2 = inv(EDGE) * inv(P1) * P2 = (P2 \ominus P1) \ominus EDGE
181  typename gst::graph_t::constraint_t::type_value DinvP1invP2 =
182  ((*P2) - (*P1)) - *EDGE_POSE;
183 
184  // Add to vector of errors:
185  errs.resize(errs.size() + 1);
186  errs.back() = gst::SE_TYPE::log(DinvP1invP2);
187 
188  // Compute the jacobians:
189  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
190  std::pair<mrpt::graphs::TPairNodeIDs, typename gst::TPairJacobs>
191  newMapEntry;
192  newMapEntry.first = ids;
193  gst::SE_TYPE::jacob_dDinvP1invP2_de1e2(
194  -(*EDGE_POSE), *P1, *P2, newMapEntry.second.first,
195  newMapEntry.second.second);
196 
197  // And insert into map of jacobians:
198  lstJacobians.insert(lstJacobians.end(), newMapEntry);
199  }
200 
201  // return overall square error: (Was:
202  // std::accumulate(...,mrpt::squareNorm_accum<>), but led to GCC
203  // errors when enabling parallelization)
204  double ret_err = 0.0;
205  for (size_t i = 0; i < errs.size(); i++)
206  ret_err += mrpt::square(errs[i].norm());
207  return ret_err;
208 }
209 
210 } // namespace graphslam
211 } // namespace mrpt
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:95
static void multiply_Jt_W_err(const JAC &J, [[maybe_unused]] const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:53
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:102
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, [[maybe_unused]] const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:74
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:131
STL namespace.
std::multimap< mrpt::graphs::TPairNodeIDs, TPairJacobs > map_pairIDs_pairJacobs_t
Auxiliary struct used in graph-slam implementation: It holds the relevant information for each of the...
SLAM methods related to graphs of pose constraints.
This base provides a set of functions for maths stuff.
static void multiply_Jt_W_err(const JAC &J, [[maybe_unused]] const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:82
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, [[maybe_unused]] const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:45
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, [[maybe_unused]] const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:38
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:139
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Auxiliary traits template for use among graph-slam problems to make life easier with these complicate...
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:123
return_t square(const num_t x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:110
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
double computeJacobiansAndErrors([[maybe_unused]] const GRAPH_T &graph, const std::vector< typename graphslam_traits< GRAPH_T >::observation_info_t > &lstObservationData, typename graphslam_traits< GRAPH_T >::map_pairIDs_pairJacobs_t &lstJacobians, std::vector< typename graphslam_traits< GRAPH_T >::Array_O > &errs)
Definition: levmarq_impl.h:152
CONTAINER::Scalar norm(const CONTAINER &v)
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, [[maybe_unused]] const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:67



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020