Main MRPT website > C++ reference for MRPT 1.5.7
levmarq_impl.h
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 #ifndef GRAPH_SLAM_LEVMARQ_IMPL_H
10 #define GRAPH_SLAM_LEVMARQ_IMPL_H
11 
13 #include <mrpt/utils/CTimeLogger.h>
15 
16 #include <memory>
17 
18 namespace mrpt
19 {
20  namespace graphslam
21  {
22  /// Internal auxiliary classes
23  namespace detail
24  {
25  using namespace mrpt;
26  using namespace mrpt::poses;
27  using namespace mrpt::graphslam;
28  using namespace mrpt::math;
29  using namespace mrpt::utils;
30  using namespace std;
31 
32  // Auxiliary struct to update the oplus increments after each iteration
33  // Specializations are below.
34  template <class POSE, class gst>
35  struct AuxPoseOPlus;
36 
37  // Nodes: CPose2D
38  template <class gst>
39  struct AuxPoseOPlus<CPose2D, gst>
40  {
41  static inline void sumIncr(CPose2D& p, const typename gst::Array_O& delta)
42  {
43  p.x_incr(delta[0]);
44  p.y_incr(delta[1]);
45  p.phi_incr(delta[2]);
46  p.normalizePhi();
47  }
48  };
49 
50  // Nodes: CPosePDFGaussianInf
51  template <class gst>
53  {
54  template <class POSE>
55  static inline void sumIncr(POSE& p, const typename gst::Array_O& delta)
56  {
57  p.x_incr(delta[0]);
58  p.y_incr(delta[1]);
59  p.phi_incr(delta[2]);
60  p.normalizePhi();
61  }
62  };
63 
64  // Nodes: CPose2D
65  template <class gst>
66  struct AuxPoseOPlus<CPose3D, gst>
67  {
68  static inline void sumIncr(CPose3D& p, const typename gst::Array_O& delta)
69  {
70  // exp_delta_i = Exp_SE( delta_i )
71  typename gst::graph_t::constraint_t::type_value exp_delta_pose(
73  gst::SE_TYPE::exp(delta, exp_delta_pose);
74  p = p + exp_delta_pose;
75  }
76  };
77 
78  // Nodes: CPosePDFGaussianInf
79  template <class gst>
81  {
82  template <class POSE>
83  static inline void sumIncr(POSE& p, const typename gst::Array_O& delta)
84  {
85  // exp_delta_i = Exp_SE( delta_i )
86  typename gst::graph_t::constraint_t::type_value exp_delta_pose(
88  gst::SE_TYPE::exp(delta, exp_delta_pose);
89  p = p + exp_delta_pose;
90  }
91  };
92 
93 
94 
95  // An auxiliary struct to compute the pseudo-ln of a pose error, possibly modified with an information matrix.
96  // Specializations are below.
97  template <class EDGE,class gst> struct AuxErrorEval;
98 
99  // For graphs of 2D constraints (no information matrix)
100  template <class gst> struct AuxErrorEval<CPose2D,gst> {
101  template <class POSE,class VEC,class EDGE_ITERATOR>
102  static inline void computePseudoLnError(const POSE &DinvP1invP2, VEC &err,
103  const EDGE_ITERATOR &edge) {
104  MRPT_UNUSED_PARAM(edge);
105  gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
106  }
107 
108  template <class MAT,class EDGE_ITERATOR>
109  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,
110  const EDGE_ITERATOR &edge) {
111  MRPT_UNUSED_PARAM(edge);
112  JtJ.multiply_AtA(J1);
113  }
114 
115  template <class MAT,class EDGE_ITERATOR>
116  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,
117  const EDGE_ITERATOR &edge) {
118  MRPT_UNUSED_PARAM(edge);
119  JtJ.multiply_AtB(J1,J2);
120  }
121 
122  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
123  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,
124  const VEC1 & ERR,VEC2 &OUT) {
125  MRPT_UNUSED_PARAM(edge);
126  J.multiply_Atb(ERR,OUT, true /* accumulate in output */ );
127  }
128  };
129 
130  // For graphs of 3D constraints (no information matrix)
131  template <class gst> struct AuxErrorEval<CPose3D,gst>
132  {
133  template <class POSE,class VEC,class EDGE_ITERATOR>
134  static inline void computePseudoLnError(const POSE &DinvP1invP2, VEC &err,
135  const EDGE_ITERATOR &edge) {
136  MRPT_UNUSED_PARAM(edge);
137  gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
138  }
139 
140  template <class MAT,class EDGE_ITERATOR>
141  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,
142  const EDGE_ITERATOR &edge) {
143  MRPT_UNUSED_PARAM(edge);
144  JtJ.multiply_AtA(J1);
145  }
146 
147  template <class MAT,class EDGE_ITERATOR>
148  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,
149  const EDGE_ITERATOR &edge) {
150  MRPT_UNUSED_PARAM(edge);
151  JtJ.multiply_AtB(J1,J2);
152  }
153 
154  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
155  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,
156  const VEC1 & ERR,VEC2 &OUT) {
157  MRPT_UNUSED_PARAM(edge);
158  J.multiply_Atb(ERR,OUT, true /* accumulate in output */ );
159  }
160  };
161 
162  // For graphs of 2D constraints (with information matrix)
163  template <class gst> struct AuxErrorEval<CPosePDFGaussianInf,gst>
164  {
165  template <class POSE,class VEC,class EDGE_ITERATOR>
166  static inline void computePseudoLnError(const POSE &DinvP1invP2, VEC &err,const EDGE_ITERATOR &edge)
167  {
168  gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
169  }
170 
171  template <class MAT,class EDGE_ITERATOR>
172  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
173  template <class MAT,class EDGE_ITERATOR>
174  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J2); }
175 
176  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
177  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
178  {
180  JtInf.multiply_AtB(J,edge->second.cov_inv);
181  JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
182  }
183  };
184 
185  // For graphs of 3D constraints (with information matrix)
186  template <class gst> struct AuxErrorEval<CPose3DPDFGaussianInf,gst> {
187  template <class POSE,class VEC,class EDGE_ITERATOR>
188  static inline void computePseudoLnError(const POSE &DinvP1invP2, VEC &err,const EDGE_ITERATOR &edge) {
189  MRPT_UNUSED_PARAM(edge);
190  gst::SE_TYPE::pseudo_ln(DinvP1invP2, err);
191  }
192 
193  template <class MAT,class EDGE_ITERATOR>
194  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
195 
196  template <class MAT,class EDGE_ITERATOR>
197  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J2); }
198 
199  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
200  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
201  {
203  JtInf.multiply_AtB(J,edge->second.cov_inv);
204  JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
205  }
206  };
207 
208  } // end NS detail
209 
210  // Compute, at once, jacobians and the error vectors for each constraint in "lstObservationData", returns the overall squared error.
211  template <class GRAPH_T>
213  const GRAPH_T &graph,
214  const std::vector<typename graphslam_traits<GRAPH_T>::observation_info_t> &lstObservationData,
216  typename mrpt::aligned_containers<typename graphslam_traits<GRAPH_T>::Array_O>::vector_t &errs
217  )
218  {
219  MRPT_UNUSED_PARAM(graph);
220  typedef graphslam_traits<GRAPH_T> gst;
221 
222  lstJacobians.clear();
223  errs.clear();
224 
225  const size_t nObservations = lstObservationData.size();
226 
227  for (size_t i=0;i<nObservations;i++)
228  {
229  const typename gst::observation_info_t & obs = lstObservationData[i];
230  typename gst::edge_const_iterator it = obs.edge;
231  const typename gst::graph_t::constraint_t::type_value* EDGE_POSE = obs.edge_mean;
232  typename gst::graph_t::constraint_t::type_value* P1 = obs.P1;
233  typename gst::graph_t::constraint_t::type_value* P2 = obs.P2;
234 
235  const mrpt::utils::TPairNodeIDs &ids = it->first;
236  const typename gst::graph_t::edge_t &edge = it->second;
237 
238  // Compute the residual pose error of these pair of nodes + its constraint:
239  // DinvP1invP2 = inv(EDGE) * inv(P1) * P2 = (P2 \ominus P1) \ominus EDGE
240  typename gst::graph_t::constraint_t::type_value DinvP1invP2 = ((*P2) - (*P1)) - *EDGE_POSE;
241 
242  // Add to vector of errors:
243  errs.resize(errs.size()+1);
245 
246  // Compute the jacobians:
247  MRPT_ALIGN16 std::pair<mrpt::utils::TPairNodeIDs,typename gst::TPairJacobs> newMapEntry;
248  newMapEntry.first = ids;
249  gst::SE_TYPE::jacobian_dDinvP1invP2_depsilon(-(*EDGE_POSE), *P1, *P2, &newMapEntry.second.first,&newMapEntry.second.second);
250 
251  // And insert into map of jacobians:
252  lstJacobians.insert(lstJacobians.end(),newMapEntry );
253  }
254 
255  // return overall square error: (Was: std::accumulate(...,mrpt::math::squareNorm_accum<>), but led to GCC errors when enabling parallelization)
256  double ret_err = 0.0;
257  for (size_t i=0;i<errs.size();i++) ret_err+=errs[i].squaredNorm();
258  return ret_err;
259  }
260 
261  } // end of NS
262 } // end of NS
263 
264 #endif
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:141
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:172
static void sumIncr(POSE &p, const typename gst::Array_O &delta)
Definition: levmarq_impl.h:83
GLuint * ids
Definition: glext.h:3767
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:174
static void computePseudoLnError(const POSE &DinvP1invP2, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:166
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:197
static void computePseudoLnError(const POSE &DinvP1invP2, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:188
STL namespace.
static void sumIncr(CPose3D &p, const typename gst::Array_O &delta)
Definition: levmarq_impl.h:68
Helper types for STL containers with Eigen memory allocators.
mrpt::aligned_containers< mrpt::utils::TPairNodeIDs, TPairJacobs >::multimap_t map_pairIDs_pairJacobs_t
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, typename mrpt::aligned_containers< typename graphslam_traits< GRAPH_T >::Array_O >::vector_t &errs)
Definition: levmarq_impl.h:212
Auxiliary struct used in graph-slam implementation: It holds the relevant information for each of the...
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:116
static void sumIncr(POSE &p, const typename gst::Array_O &delta)
Definition: levmarq_impl.h:55
SLAM methods related to graphs of pose constraints.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:123
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
static void computePseudoLnError(const POSE &DinvP1invP2, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:134
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:200
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
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:194
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:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:109
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:148
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:177
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:155
static void computePseudoLnError(const POSE &DinvP1invP2, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:102
GLfloat GLfloat p
Definition: glext.h:5587
#define MRPT_ALIGN16
static void sumIncr(CPose2D &p, const typename gst::Array_O &delta)
Definition: levmarq_impl.h:41



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