MRPT  1.9.9
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-2019, 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, const EDGE_ITERATOR& edge)
40  {
41  MRPT_UNUSED_PARAM(edge);
42  JtJ.matProductOf_AtA(J1);
43  }
44 
45  template <class MAT, class EDGE_ITERATOR>
46  static inline void multiplyJ1tLambdaJ2(
47  const MAT& J1, const MAT& J2, MAT& JtJ, const EDGE_ITERATOR& edge)
48  {
49  MRPT_UNUSED_PARAM(edge);
50  JtJ.asEigen() = J1.transpose() * J2.asEigen();
51  }
52 
53  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
54  static inline void multiply_Jt_W_err(
55  const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
56  {
57  MRPT_UNUSED_PARAM(edge);
58  const auto grad_incr = (J.transpose() * ERR.asEigen()).eval();
59  OUT.asEigen() += grad_incr;
60  }
61 };
62 
63 // For graphs of 3D constraints (no information matrix)
64 template <class gst>
65 struct AuxErrorEval<CPose3D, gst>
66 {
67  template <class MAT, class EDGE_ITERATOR>
68  static inline void multiplyJtLambdaJ(
69  const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge)
70  {
71  MRPT_UNUSED_PARAM(edge);
72  JtJ.matProductOf_AtA(J1);
73  }
74 
75  template <class MAT, class EDGE_ITERATOR>
76  static inline void multiplyJ1tLambdaJ2(
77  const MAT& J1, const MAT& J2, MAT& JtJ, const EDGE_ITERATOR& edge)
78  {
79  MRPT_UNUSED_PARAM(edge);
80  JtJ.asEigen() = J1.transpose() * J2.asEigen();
81  }
82 
83  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
84  static inline void multiply_Jt_W_err(
85  const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
86  {
87  MRPT_UNUSED_PARAM(edge);
88  OUT.asEigen() += J.transpose() * ERR.asEigen();
89  }
90 };
91 
92 // For graphs of 2D constraints (with information matrix)
93 template <class gst>
95 {
96  template <class MAT, class EDGE_ITERATOR>
97  static inline void multiplyJtLambdaJ(
98  const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge)
99  {
100  JtJ.asEigen() =
101  (J1.transpose() * edge->second.cov_inv.asEigen()) * J1.asEigen();
102  }
103  template <class MAT, class EDGE_ITERATOR>
104  static inline void multiplyJ1tLambdaJ2(
105  const MAT& J1, const MAT& J2, MAT& JtJ, const EDGE_ITERATOR& edge)
106  {
107  JtJ.asEigen() =
108  (J1.transpose() * edge->second.cov_inv.asEigen()) * J2.asEigen();
109  }
110 
111  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
112  static inline void multiply_Jt_W_err(
113  const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
114  {
115  OUT.asEigen() +=
116  (J.transpose() * edge->second.cov_inv.asEigen()) * ERR.asEigen();
117  }
118 };
119 
120 // For graphs of 3D constraints (with information matrix)
121 template <class gst>
123 {
124  template <class MAT, class EDGE_ITERATOR>
125  static inline void multiplyJtLambdaJ(
126  const MAT& J1, MAT& JtJ, const EDGE_ITERATOR& edge)
127  {
128  JtJ.asEigen() =
129  (J1.transpose() * edge->second.cov_inv.asEigen()) * J1.asEigen();
130  }
131 
132  template <class MAT, class EDGE_ITERATOR>
133  static inline void multiplyJ1tLambdaJ2(
134  const MAT& J1, const MAT& J2, MAT& JtJ, const EDGE_ITERATOR& edge)
135  {
136  JtJ.asEigen() =
137  (J1.transpose() * edge->second.cov_inv.asEigen()) * J2.asEigen();
138  }
139 
140  template <class JAC, class EDGE_ITERATOR, class VEC1, class VEC2>
141  static inline void multiply_Jt_W_err(
142  const JAC& J, const EDGE_ITERATOR& edge, const VEC1& ERR, VEC2& OUT)
143  {
144  OUT.asEigen() +=
145  (J.transpose() * edge->second.cov_inv.asEigen()) * ERR.asEigen();
146  }
147 };
148 
149 } // namespace detail
150 
151 // Compute, at once, jacobians and the error vectors for each constraint in
152 // "lstObservationData", returns the overall squared error.
153 template <class GRAPH_T>
155  const GRAPH_T& graph,
156  const std::vector<typename graphslam_traits<GRAPH_T>::observation_info_t>&
157  lstObservationData,
159  std::vector<typename graphslam_traits<GRAPH_T>::Array_O>& errs)
160 {
161  MRPT_UNUSED_PARAM(graph);
162  using gst = graphslam_traits<GRAPH_T>;
163 
164  lstJacobians.clear();
165  errs.clear();
166 
167  const size_t nObservations = lstObservationData.size();
168 
169  for (size_t i = 0; i < nObservations; i++)
170  {
171  const typename gst::observation_info_t& obs = lstObservationData[i];
172  auto e = obs.edge;
173  const typename gst::graph_t::constraint_t::type_value* EDGE_POSE =
174  obs.edge_mean;
175  typename gst::graph_t::constraint_t::type_value* P1 = obs.P1;
176  typename gst::graph_t::constraint_t::type_value* P2 = obs.P2;
177 
178  const auto& ids = e->first;
179  // const auto& edge = e->second;
180 
181  // Compute the residual pose error of these pair of nodes + its
182  // constraint:
183  // DinvP1invP2 = inv(EDGE) * inv(P1) * P2 = (P2 \ominus P1) \ominus EDGE
184  typename gst::graph_t::constraint_t::type_value DinvP1invP2 =
185  ((*P2) - (*P1)) - *EDGE_POSE;
186 
187  // Add to vector of errors:
188  errs.resize(errs.size() + 1);
189  errs.back() = gst::SE_TYPE::log(DinvP1invP2);
190 
191  // Compute the jacobians:
192  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
193  std::pair<mrpt::graphs::TPairNodeIDs, typename gst::TPairJacobs>
194  newMapEntry;
195  newMapEntry.first = ids;
196  gst::SE_TYPE::jacob_dDinvP1invP2_de1e2(
197  -(*EDGE_POSE), *P1, *P2, newMapEntry.second.first,
198  newMapEntry.second.second);
199 
200  // And insert into map of jacobians:
201  lstJacobians.insert(lstJacobians.end(), newMapEntry);
202  }
203 
204  // return overall square error: (Was:
205  // std::accumulate(...,mrpt::squareNorm_accum<>), but led to GCC
206  // errors when enabling parallelization)
207  double ret_err = 0.0;
208  for (size_t i = 0; i < errs.size(); i++)
209  ret_err += mrpt::square(errs[i].norm());
210  return ret_err;
211 }
212 
213 } // namespace graphslam
214 } // namespace mrpt
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:68
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:97
GLuint * ids
Definition: glext.h:3916
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:104
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:133
double computeJacobiansAndErrors(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:154
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...
T square(const T x)
Inline function for the square of a number.
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:46
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, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:54
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:141
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:125
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:84
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:38
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:76
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:112
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:84
CONTAINER::Scalar norm(const CONTAINER &v)
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019