Main MRPT website > C++ reference
MRPT logo
levmarq_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef GRAPH_SLAM_LEVMARQ_IMPL_H
29 #define GRAPH_SLAM_LEVMARQ_IMPL_H
30 
31 #include <mrpt/graphs/CNetworkOfPoses.h>
32 #include <mrpt/utils/CTimeLogger.h>
33 #include <mrpt/math/CSparseMatrix.h>
34 
35 #include <memory>
36 
37 namespace mrpt
38 {
39  namespace graphslam
40  {
41  using namespace mrpt;
42  using namespace mrpt::poses;
43  using namespace mrpt::graphslam;
44  using namespace mrpt::math;
45  using namespace mrpt::utils;
46  using namespace std;
47 
48  /// Internal auxiliary classes
49  namespace detail
50  {
51  // An auxiliary struct to compute the pseudo-ln of a pose error, possibly modified with an information matrix.
52  // Specializations are below.
53  template <class EDGE,class gst> struct AuxErrorEval;
54 
55  // For graphs of 2D constraints (no information matrix)
56  template <class gst> struct AuxErrorEval<CPose2D,gst> {
57  template <class POSE,class VEC,class EDGE_ITERATOR>
58  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge) { gst::SE_TYPE::pseudo_ln(P1DP2inv, err); }
59 
60  template <class MAT,class EDGE_ITERATOR>
61  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtA(J1); }
62 
63  template <class MAT,class EDGE_ITERATOR>
64  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtB(J1,J2); }
65 
66  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
67  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT) { J.multiply_Atb(ERR,OUT, true /* accumulate in output */ ); }
68  };
69 
70  // For graphs of 3D constraints (no information matrix)
71  template <class gst> struct AuxErrorEval<CPose3D,gst> {
72  template <class POSE,class VEC,class EDGE_ITERATOR>
73  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge) { gst::SE_TYPE::pseudo_ln(P1DP2inv, err); }
74 
75  template <class MAT,class EDGE_ITERATOR>
76  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtA(J1); }
77 
78  template <class MAT,class EDGE_ITERATOR>
79  static inline void multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtB(J1,J2); }
80 
81  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
82  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT) { J.multiply_Atb(ERR,OUT, true /* accumulate in output */ ); }
83  };
84 
85  // For graphs of 2D constraints (with information matrix)
86  template <class gst> struct AuxErrorEval<CPosePDFGaussianInf,gst> {
87  template <class POSE,class VEC,class EDGE_ITERATOR>
88  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge)
89  {
90  gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
91  }
92 
93  template <class MAT,class EDGE_ITERATOR>
94  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
95  template <class MAT,class EDGE_ITERATOR>
96  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); }
97 
98  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
99  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
100  {
101  CMatrixDouble33 JtInf(UNINITIALIZED_MATRIX);
102  JtInf.multiply_AtB(J,edge->second.cov_inv);
103  JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
104  }
105  };
106 
107  // For graphs of 3D constraints (with information matrix)
108  template <class gst> struct AuxErrorEval<CPose3DPDFGaussianInf,gst> {
109  template <class POSE,class VEC,class EDGE_ITERATOR>
110  static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge) { gst::SE_TYPE::pseudo_ln(P1DP2inv, err); }
111 
112  template <class MAT,class EDGE_ITERATOR>
113  static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
114 
115  template <class MAT,class EDGE_ITERATOR>
116  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); }
117 
118  template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
119  static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
120  {
121  CMatrixDouble66 JtInf(UNINITIALIZED_MATRIX);
122  JtInf.multiply_AtB(J,edge->second.cov_inv);
123  JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
124  }
125  };
126 
127  } // end NS detail
128 
129  // Compute, at once, jacobians and the error vectors for each constraint in "lstObservationData", returns the overall squared error.
130  template <class GRAPH_T>
131  double computeJacobiansAndErrors(
132  const GRAPH_T &graph,
133  const vector<typename graphslam_traits<GRAPH_T>::observation_info_t> &lstObservationData,
134  typename graphslam_traits<GRAPH_T>::map_pairIDs_pairJacobs_t &lstJacobians,
135  typename mrpt::aligned_containers<typename graphslam_traits<GRAPH_T>::Array_O>::vector_t &errs
136  )
137  {
138  typedef graphslam_traits<GRAPH_T> gst;
139 
140  lstJacobians.clear();
141  errs.clear();
142 
143  const size_t nObservations = lstObservationData.size();
144 
145  for (size_t i=0;i<nObservations;i++)
146  {
147  const typename gst::observation_info_t & obs = lstObservationData[i];
148  typename gst::edge_const_iterator it = obs.edge;
149  const typename gst::graph_t::constraint_t::type_value* EDGE_POSE = obs.edge_mean;
150  typename gst::graph_t::constraint_t::type_value* P1 = obs.P1;
151  typename gst::graph_t::constraint_t::type_value* P2 = obs.P2;
152 
153  const TPairNodeIDs &ids = it->first;
154  const typename gst::graph_t::edge_t &edge = it->second;
155 
156  // Compute the residual pose error of these pair of nodes + its constraint,
157  // that is: P1DP2inv = P1 * EDGE * inv(P2)
158  typename gst::graph_t::constraint_t::type_value P1DP2inv(UNINITIALIZED_POSE);
159  {
160  typename gst::graph_t::constraint_t::type_value P1D(UNINITIALIZED_POSE);
161  P1D.composeFrom(*P1,*EDGE_POSE);
162  const typename gst::graph_t::constraint_t::type_value P2inv = -(*P2); // Pose inverse (NOT just switching signs!)
163  P1DP2inv.composeFrom(P1D,P2inv);
164  }
165 
166  // Add to vector of errors:
167  errs.resize(errs.size()+1);
168  detail::AuxErrorEval<typename gst::edge_t,gst>::computePseudoLnError(P1DP2inv, errs.back(),edge);
169 
170  // Compute the jacobians:
171  EIGEN_ALIGN16 std::pair<TPairNodeIDs,typename gst::TPairJacobs> newMapEntry;
172  newMapEntry.first = ids;
173  gst::SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &newMapEntry.second.first,&newMapEntry.second.second);
174 
175  // And insert into map of jacobians:
176  lstJacobians.insert(lstJacobians.end(),newMapEntry );
177  }
178 
179  // return overall square error:
180  return std::accumulate( errs.begin(), errs.end(),0.0, mrpt::math::squareNorm_accum<typename gst::Array_O> );
181  }
182 
183  } // end of NS
184 } // end of NS
185 
186 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013