Main MRPT website > C++ reference for MRPT 1.5.5
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 
12 #include <mrpt/graphs/CNetworkOfPoses.h>
13 #include <mrpt/utils/CTimeLogger.h>
14 #include <mrpt/math/CSparseMatrix.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  // An auxiliary struct to compute the pseudo-ln of a pose error, possibly modified with an information matrix.
33  // Specializations are below.
34  template <class EDGE,class gst> struct AuxErrorEval;
35 
36  // For graphs of 2D constraints (no information matrix)
37  template <class gst> struct AuxErrorEval<CPose2D,gst> {
38  template <class POSE,class VEC,class EDGE_ITERATOR>
39  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,
40  const EDGE_ITERATOR &edge) {
41  MRPT_UNUSED_PARAM(edge);
42  gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
43  }
44 
45  template <class MAT,class EDGE_ITERATOR>
46  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,
47  const EDGE_ITERATOR &edge) {
48  MRPT_UNUSED_PARAM(edge);
49  JtJ.multiply_AtA(J1);
50  }
51 
52  template <class MAT,class EDGE_ITERATOR>
53  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,
54  const EDGE_ITERATOR &edge) {
55  MRPT_UNUSED_PARAM(edge);
56  JtJ.multiply_AtB(J1,J2);
57  }
58 
59  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
60  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,
61  const VEC1 & ERR,VEC2 &OUT) {
62  MRPT_UNUSED_PARAM(edge);
63  J.multiply_Atb(ERR,OUT, true /* accumulate in output */ );
64  }
65  };
66 
67  // For graphs of 3D constraints (no information matrix)
68  template <class gst> struct AuxErrorEval<CPose3D,gst>
69  {
70  template <class POSE,class VEC,class EDGE_ITERATOR>
71  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,
72  const EDGE_ITERATOR &edge) {
73  MRPT_UNUSED_PARAM(edge);
74  gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
75  }
76 
77  template <class MAT,class EDGE_ITERATOR>
78  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,
79  const EDGE_ITERATOR &edge) {
80  MRPT_UNUSED_PARAM(edge);
81  JtJ.multiply_AtA(J1);
82  }
83 
84  template <class MAT,class EDGE_ITERATOR>
85  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,
86  const EDGE_ITERATOR &edge) {
87  MRPT_UNUSED_PARAM(edge);
88  JtJ.multiply_AtB(J1,J2);
89  }
90 
91  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
92  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,
93  const VEC1 & ERR,VEC2 &OUT) {
94  MRPT_UNUSED_PARAM(edge);
95  J.multiply_Atb(ERR,OUT, true /* accumulate in output */ );
96  }
97  };
98 
99  // For graphs of 2D constraints (with information matrix)
100  template <class gst> struct AuxErrorEval<CPosePDFGaussianInf,gst>
101  {
102  template <class POSE,class VEC,class EDGE_ITERATOR>
103  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge)
104  {
105  gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
106  }
107 
108  template <class MAT,class EDGE_ITERATOR>
109  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
110  template <class MAT,class EDGE_ITERATOR>
111  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); }
112 
113  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
114  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
115  {
116  mrpt::math::CMatrixDouble33 JtInf(mrpt::math::UNINITIALIZED_MATRIX);
117  JtInf.multiply_AtB(J,edge->second.cov_inv);
118  JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
119  }
120  };
121 
122  // For graphs of 3D constraints (with information matrix)
123  template <class gst> struct AuxErrorEval<CPose3DPDFGaussianInf,gst> {
124  template <class POSE,class VEC,class EDGE_ITERATOR>
125  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge) {
126  MRPT_UNUSED_PARAM(edge);
127  gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
128  }
129 
130  template <class MAT,class EDGE_ITERATOR>
131  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
132 
133  template <class MAT,class EDGE_ITERATOR>
134  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); }
135 
136  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
137  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
138  {
139  mrpt::math::CMatrixDouble66 JtInf(mrpt::math::UNINITIALIZED_MATRIX);
140  JtInf.multiply_AtB(J,edge->second.cov_inv);
141  JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
142  }
143  };
144 
145  } // end NS detail
146 
147  // Compute, at once, jacobians and the error vectors for each constraint in "lstObservationData", returns the overall squared error.
148  template <class GRAPH_T>
149  double computeJacobiansAndErrors(
150  const GRAPH_T &graph,
151  const std::vector<typename graphslam_traits<GRAPH_T>::observation_info_t> &lstObservationData,
152  typename graphslam_traits<GRAPH_T>::map_pairIDs_pairJacobs_t &lstJacobians,
153  typename mrpt::aligned_containers<typename graphslam_traits<GRAPH_T>::Array_O>::vector_t &errs
154  )
155  {
156  MRPT_UNUSED_PARAM(graph);
157  typedef graphslam_traits<GRAPH_T> gst;
158 
159  lstJacobians.clear();
160  errs.clear();
161 
162  const size_t nObservations = lstObservationData.size();
163 
164  for (size_t i=0;i<nObservations;i++)
165  {
166  const typename gst::observation_info_t & obs = lstObservationData[i];
167  typename gst::edge_const_iterator it = obs.edge;
168  const typename gst::graph_t::constraint_t::type_value* EDGE_POSE = obs.edge_mean;
169  typename gst::graph_t::constraint_t::type_value* P1 = obs.P1;
170  typename gst::graph_t::constraint_t::type_value* P2 = obs.P2;
171 
172  const mrpt::utils::TPairNodeIDs &ids = it->first;
173  const typename gst::graph_t::edge_t &edge = it->second;
174 
175  // Compute the residual pose error of these pair of nodes + its constraint,
176  // that is: P1DP2inv = P1 * EDGE * inv(P2)
177  typename gst::graph_t::constraint_t::type_value P1DP2inv(mrpt::poses::UNINITIALIZED_POSE);
178  {
179  typename gst::graph_t::constraint_t::type_value P1D(mrpt::poses::UNINITIALIZED_POSE);
180  P1D.composeFrom(*P1,*EDGE_POSE);
181  const typename gst::graph_t::constraint_t::type_value P2inv = -(*P2); // Pose inverse (NOT just switching signs!)
182  P1DP2inv.composeFrom(P1D,P2inv);
183  }
184 
185  // Add to vector of errors:
186  errs.resize(errs.size()+1);
187  detail::AuxErrorEval<typename gst::edge_t,gst>::computePseudoLnError(P1DP2inv, errs.back(),edge);
188 
189  // Compute the jacobians:
190  MRPT_ALIGN16 std::pair<mrpt::utils::TPairNodeIDs,typename gst::TPairJacobs> newMapEntry;
191  newMapEntry.first = ids;
192  gst::SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &newMapEntry.second.first,&newMapEntry.second.second);
193 
194  // And insert into map of jacobians:
195  lstJacobians.insert(lstJacobians.end(),newMapEntry );
196  }
197 
198  // return overall square error: (Was: std::accumulate(...,mrpt::math::squareNorm_accum<>), but led to GCC errors when enabling parallelization)
199  double ret_err = 0.0;
200  for (size_t i=0;i<errs.size();i++) ret_err+=errs[i].squaredNorm();
201  return ret_err;
202  }
203 
204  } // end of NS
205 } // end of NS
206 
207 #endif
mrpt::graphslam::detail::AuxErrorEval< CPose3D, gst >::multiplyJtLambdaJ
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:78
mrpt::graphslam::detail::AuxErrorEval< CPosePDFGaussianInf, gst >::multiplyJtLambdaJ
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:109
ids
GLuint * ids
Definition: glext.h:3767
mrpt::graphslam::detail::AuxErrorEval< CPosePDFGaussianInf, gst >::computePseudoLnError
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:103
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::graphslam::detail::AuxErrorEval< CPosePDFGaussianInf, gst >::multiplyJ1tLambdaJ2
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:111
CSparseMatrix.h
mrpt::graphslam::detail::AuxErrorEval< CPose3DPDFGaussianInf, gst >::multiplyJ1tLambdaJ2
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:134
std
STL namespace.
mrpt::aligned_containers
Helper types for STL containers with Eigen memory allocators.
Definition: aligned_containers.h:25
mrpt::graphslam::detail::AuxErrorEval< CPose2D, gst >::computePseudoLnError
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:39
mrpt::graphslam::graphslam_traits::map_pairIDs_pairJacobs_t
mrpt::aligned_containers< mrpt::utils::TPairNodeIDs, TPairJacobs >::multimap_t map_pairIDs_pairJacobs_t
Definition: graphslam/include/mrpt/graphslam/types.h:42
mrpt::graphslam::computeJacobiansAndErrors
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:149
mrpt::graphslam::graphslam_traits::observation_info_t
Auxiliary struct used in graph-slam implementation: It holds the relevant information for each of the...
Definition: graphslam/include/mrpt/graphslam/types.h:45
mrpt::graphslam::detail::AuxErrorEval< CPose2D, gst >::multiplyJ1tLambdaJ2
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:53
mrpt::graphslam::detail::AuxErrorEval< CPose3D, gst >::computePseudoLnError
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:71
mrpt::graphslam
SLAM methods related to graphs of pose constraints.
Definition: TUserOptionsChecker.h:32
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::graphslam::detail::AuxErrorEval< CPose2D, gst >::multiply_Jt_W_err
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:60
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::mrpt::utils::TPairNodeIDs
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: maps/CColouredPointsMap.h:46
mrpt::graphslam::detail::AuxErrorEval< CPose3DPDFGaussianInf, gst >::multiply_Jt_W_err
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:137
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::graphslam::graphslam_traits
Auxiliary traits template for use among graph-slam problems to make life easier with these complicate...
Definition: graphslam/include/mrpt/graphslam/types.h:29
mrpt::graphslam::detail::AuxErrorEval< CPose3DPDFGaussianInf, gst >::multiplyJtLambdaJ
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:131
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::poses::CPosePDFGaussianInf
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
Definition: CPosePDFGaussianInf.h:35
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::graphslam::detail::AuxErrorEval< CPose2D, gst >::multiplyJtLambdaJ
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:46
mrpt::graphslam::detail::AuxErrorEval< CPose3D, gst >::multiplyJ1tLambdaJ2
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:85
mrpt::graphslam::detail::AuxErrorEval< CPosePDFGaussianInf, gst >::multiply_Jt_W_err
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:114
mrpt::graphslam::graphslam_traits::Array_O
SE_TYPE::array_t Array_O
Definition: graphslam/include/mrpt/graphslam/types.h:37
mrpt::poses::CPose3DPDFGaussianInf
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
Definition: CPose3DPDFGaussianInf.h:38
CTimeLogger.h
mrpt::graphslam::detail::AuxErrorEval< CPose3DPDFGaussianInf, gst >::computePseudoLnError
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:125
mrpt::graphslam::detail::AuxErrorEval< CPose3D, gst >::multiply_Jt_W_err
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:92
MRPT_ALIGN16
#define MRPT_ALIGN16
Definition: maps/CColouredPointsMap.h:138
CNetworkOfPoses.h


Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019