levmarq_impl.h Source File

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                       http://www.mrpt.org/                                |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef GRAPH_SLAM_LEVMARQ_IMPL_H
00029 #define GRAPH_SLAM_LEVMARQ_IMPL_H
00030 
00031 #include <mrpt/graphs/CNetworkOfPoses.h>
00032 #include <mrpt/utils/CTimeLogger.h>
00033 #include <mrpt/math/CSparseMatrix.h>
00034 
00035 #include <memory>
00036 
00037 namespace mrpt
00038 {
00039         namespace graphslam
00040         {
00041                 using namespace mrpt;
00042                 using namespace mrpt::poses;
00043                 using namespace mrpt::graphslam;
00044                 using namespace mrpt::math;
00045                 using namespace mrpt::utils;
00046                 using namespace std;
00047 
00048                 /// Internal auxiliary classes
00049                 namespace detail 
00050                 {
00051                         // An auxiliary struct to compute the pseudo-ln of a pose error, possibly modified with an information matrix.
00052                         //  Specializations are below.
00053                         template <class EDGE,class gst> struct AuxErrorEval;
00054 
00055                         // For graphs of 2D constraints (no information matrix)
00056                         template <class gst> struct AuxErrorEval<CPose2D,gst> {
00057                                 template <class POSE,class VEC,class EDGE_ITERATOR>
00058                                 static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge) { gst::SE_TYPE::pseudo_ln(P1DP2inv, err); }
00059 
00060                                 template <class MAT,class EDGE_ITERATOR>
00061                                 static inline void      multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtA(J1);  }
00062 
00063                                 template <class MAT,class EDGE_ITERATOR>
00064                                 static inline void      multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtB(J1,J2); }
00065 
00066                                 template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
00067                                 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 */ ); }
00068                         };
00069 
00070                         // For graphs of 3D constraints (no information matrix)
00071                         template <class gst> struct AuxErrorEval<CPose3D,gst> {
00072                                 template <class POSE,class VEC,class EDGE_ITERATOR>
00073                                 static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge) { gst::SE_TYPE::pseudo_ln(P1DP2inv, err); }
00074 
00075                                 template <class MAT,class EDGE_ITERATOR>
00076                                 static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtA(J1); }
00077 
00078                                 template <class MAT,class EDGE_ITERATOR>
00079                                 static inline void      multiplyJ1tLambdaJ2(const MAT &J1, const MAT &J2, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtB(J1,J2); }
00080 
00081                                 template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
00082                                 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 */ ); }
00083                         };
00084 
00085                         // For graphs of 2D constraints (with information matrix)
00086                         template <class gst> struct AuxErrorEval<CPosePDFGaussianInf,gst> {
00087                                 template <class POSE,class VEC,class EDGE_ITERATOR>
00088                                 static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge)
00089                                 {
00090                                         gst::SE_TYPE::pseudo_ln(P1DP2inv, err);
00091                                 }
00092 
00093                                 template <class MAT,class EDGE_ITERATOR>
00094                                 static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
00095                                 template <class MAT,class EDGE_ITERATOR>
00096                                 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); }
00097 
00098                                 template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
00099                                 static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
00100                                 {
00101                                         CMatrixDouble33 JtInf(UNINITIALIZED_MATRIX);
00102                                         JtInf.multiply_AtB(J,edge->second.cov_inv);
00103                                         JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
00104                                 }
00105                         };
00106 
00107                         // For graphs of 3D constraints (with information matrix)
00108                         template <class gst> struct AuxErrorEval<CPose3DPDFGaussianInf,gst> {
00109                                 template <class POSE,class VEC,class EDGE_ITERATOR>
00110                                 static inline void computePseudoLnError(const POSE &P1DP2inv, VEC &err,const EDGE_ITERATOR &edge) { gst::SE_TYPE::pseudo_ln(P1DP2inv, err); }
00111 
00112                                 template <class MAT,class EDGE_ITERATOR>
00113                                 static inline void multiplyJtLambdaJ(const MAT &J1, MAT &JtJ,const EDGE_ITERATOR &edge) { JtJ.multiply_AtBC(J1,edge->second.cov_inv,J1); }
00114 
00115                                 template <class MAT,class EDGE_ITERATOR>
00116                                 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); }
00117 
00118                                 template <class JAC,class EDGE_ITERATOR,class VEC1,class VEC2>
00119                                 static inline void multiply_Jt_W_err(const JAC &J,const EDGE_ITERATOR &edge,const VEC1 & ERR,VEC2 &OUT)
00120                                 {
00121                                         CMatrixDouble66 JtInf(UNINITIALIZED_MATRIX);
00122                                         JtInf.multiply_AtB(J,edge->second.cov_inv);
00123                                         JtInf.multiply_Ab(ERR,OUT, true /* accumulate in output */ );
00124                                 }
00125                         };
00126 
00127                 } // end NS detail
00128 
00129                 // Compute, at once, jacobians and the error vectors for each constraint in "lstObservationData", returns the overall squared error.
00130                 template <class GRAPH_T>
00131                 double computeJacobiansAndErrors(
00132                         const GRAPH_T &graph,
00133                         const vector<typename graphslam_traits<GRAPH_T>::observation_info_t>  &lstObservationData,
00134                         typename graphslam_traits<GRAPH_T>::map_pairIDs_pairJacobs_t   &lstJacobians,
00135                         typename mrpt::aligned_containers<typename graphslam_traits<GRAPH_T>::Array_O>::vector_t &errs
00136                         )
00137                 {
00138                         typedef graphslam_traits<GRAPH_T> gst;
00139 
00140                         lstJacobians.clear();
00141                         errs.clear();
00142 
00143                         const size_t nObservations = lstObservationData.size();
00144 
00145                         for (size_t i=0;i<nObservations;i++)
00146                         {
00147                                 const typename gst::observation_info_t & obs = lstObservationData[i];
00148                                 typename gst::edge_const_iterator it = obs.edge;
00149                                 const typename gst::graph_t::constraint_t::type_value* EDGE_POSE = obs.edge_mean;
00150                                 typename gst::graph_t::constraint_t::type_value* P1 = obs.P1;
00151                                 typename gst::graph_t::constraint_t::type_value* P2 = obs.P2;
00152 
00153                                 const TPairNodeIDs                   &ids  = it->first;
00154                                 const typename gst::graph_t::edge_t  &edge = it->second;
00155 
00156                                 // Compute the residual pose error of these pair of nodes + its constraint,
00157                                 //  that is: P1DP2inv = P1 * EDGE * inv(P2)
00158                                 typename gst::graph_t::constraint_t::type_value P1DP2inv(UNINITIALIZED_POSE);
00159                                 {
00160                                         typename gst::graph_t::constraint_t::type_value P1D(UNINITIALIZED_POSE);
00161                                         P1D.composeFrom(*P1,*EDGE_POSE);
00162                                         const typename gst::graph_t::constraint_t::type_value P2inv = -(*P2); // Pose inverse (NOT just switching signs!)
00163                                         P1DP2inv.composeFrom(P1D,P2inv);
00164                                 }
00165 
00166                                 // Add to vector of errors:
00167                                 errs.resize(errs.size()+1);
00168                                 detail::AuxErrorEval<typename gst::edge_t,gst>::computePseudoLnError(P1DP2inv, errs.back(),edge);
00169 
00170                                 // Compute the jacobians:
00171                                 EIGEN_ALIGN16 std::pair<TPairNodeIDs,typename gst::TPairJacobs> newMapEntry;
00172                                 newMapEntry.first = ids;
00173                                 gst::SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &newMapEntry.second.first,&newMapEntry.second.second);
00174 
00175                                 // And insert into map of jacobians:
00176                                 lstJacobians.insert(lstJacobians.end(),newMapEntry );
00177                         }
00178 
00179                         // return overall square error:
00180                         return std::accumulate( errs.begin(), errs.end(),0.0, mrpt::math::squareNorm_accum<typename gst::Array_O> );
00181                 }
00182 
00183         } // end of NS
00184 } // end of NS
00185 
00186 #endif