Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
15
16#include <memory>
17
18namespace 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) {
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) {
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) {
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) {
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) {
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) {
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) {
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) {
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 {
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 {
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>
150 const GRAPH_T &graph,
151 const std::vector<typename graphslam_traits<GRAPH_T>::observation_info_t> &lstObservationData,
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);
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
A numeric matrix of compile-time fixed size.
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: types_simple.h:46
#define MRPT_ALIGN16
Definition: mrpt_macros.h:92
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
SLAM methods related to graphs of pose constraints.
Definition: levmarq.h:22
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
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
@ UNINITIALIZED_POSE
Definition: CPoseOrPoint.h:35
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: zip.h:16
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
Helper types for STL containers with Eigen memory allocators.
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:60
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:39
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:53
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:46
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:71
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:78
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:85
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:92
static void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:134
static void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:131
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:137
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:125
static void computePseudoLnError(const POSE &P1DP2inv, VEC &err, const EDGE_ITERATOR &edge)
Definition: levmarq_impl.h:103
static void multiply_Jt_W_err(const JAC &J, const EDGE_ITERATOR &edge, const VEC1 &ERR, VEC2 &OUT)
Definition: levmarq_impl.h:114
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:111
Auxiliary struct used in graph-slam implementation: It holds the relevant information for each of the...
Auxiliary traits template for use among graph-slam problems to make life easier with these complicate...
mrpt::aligned_containers< mrpt::utils::TPairNodeIDs, TPairJacobs >::multimap_t map_pairIDs_pairJacobs_t



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 06:31:24 UTC 2023