Main MRPT website > C++ reference
MRPT logo

graph_slam.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
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_H
00029 #define GRAPH_SLAM_H
00030 
00031 #include <mrpt/poses/CNetworkOfPoses.h>
00032 #include <mrpt/poses/SE_traits.h>
00033 
00034 #include <mrpt/slam/link_pragmas.h>
00035 
00036 namespace mrpt
00037 {
00038         /** SLAM methods related to graphs of pose constraints
00039           * \sa mrpt::poses::CNetworkOfPoses
00040           */
00041         namespace graphslam
00042         {
00043                 /** Auxiliary traits template for use among graph-slam problems to make life easier with these complicated, long data type names */
00044                 template <class EDGE_TYPE, class MAPS_IMPLEMENTATION>
00045                 struct graphslam_traits
00046                 {
00047                         typedef mrpt::poses::CNetworkOfPoses<EDGE_TYPE,MAPS_IMPLEMENTATION>  graph_t;
00048                         typedef typename graph_t::edges_map_t::const_iterator   edge_const_iterator;
00049                         typedef EDGE_TYPE                                       edge_t;
00050                         typedef typename EDGE_TYPE::type_value                  edge_poses_type;
00051                         typedef mrpt::poses::SE_traits<edge_poses_type::rotation_dimensions> SE_TYPE;
00052                         typedef typename SE_TYPE::matrix_VxV_t                  matrix_VxV_t;
00053                         typedef typename SE_TYPE::array_t                       Array_O; // An array of the correct size for an "observation" (i.e. a relative pose in an edge)
00054                         typedef std::pair<matrix_VxV_t,matrix_VxV_t>            TPairJacobs;
00055                         typedef typename mrpt::aligned_containers<
00056                                 mrpt::utils::TPairNodeIDs,
00057                                 TPairJacobs
00058                                 >::map_t  map_pairIDs_pairJacobs_t;
00059 
00060                         /** Auxiliary struct used in graph-slam implementation: It holds the relevant information for each of the constraints being taking into account. */
00061                         struct observation_info_t
00062                         {
00063                                 typedef graphslam_traits<EDGE_TYPE,MAPS_IMPLEMENTATION> gst;
00064                                 // Data:
00065                                 typename gst::edge_const_iterator                     edge;
00066                                 const typename gst::graph_t::constraint_t::type_value *edge_mean;
00067                                 typename gst::graph_t::constraint_t::type_value       *P1,*P2;
00068                         };
00069 
00070                         typedef void (*TFunctorFeedback)(const typename graphslam_traits<EDGE_TYPE,MAPS_IMPLEMENTATION>::graph_t &graph, const size_t iter, const size_t max_iter, const double cur_sq_error );
00071                 };
00072 
00073                 /** Output information for mrpt::graphslam::optimize_graph_spa_levmarq() */
00074                 struct TResultInfoSpaLevMarq
00075                 {
00076                         size_t  num_iters;             //!< The number of LM iterations executed.
00077                         double  final_total_sq_error;  //!< The sum of all the squared errors for every constraint involved in the problem.
00078                 };
00079 
00080 
00081                 /** Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and a Levenberg-Marquartd optimizer.
00082                   *  This method works for all types of graphs derived from \a CNetworkOfPoses (see its reference mrpt::poses::CNetworkOfPoses for the list).
00083                   *  The input data are all the pose constraints in \a graph (graph.edges), and the gross first estimations of the "global" pose coordinates (in graph.nodes).
00084                   *
00085                   *  Note that these first coordinates can be obtained with mrpt::poses::CNetworkOfPoses::dijkstra_nodes_estimate().
00086                   *
00087                   * The method implemented in this file is based on this work:
00088                   *  - "Efficient Sparse Pose Adjustment for 2D Mapping", Kurt Konolige et al., 2010.
00089                   * , but generalized for not only 2D but 2D and 3D poses, and using on-manifold optimization.
00090                   *
00091                   * \param[in,out] graph The input edges and output poses.
00092                   * \param[out] out_info Some basic output information on the process.
00093                   * \param[in] nodes_to_optimize The list of nodes whose global poses are to be optimized. If NULL (default), all the node IDs are optimized (but that marked as \a root in the graph).
00094                   * \param[in] extra_params Optional parameters, see below.
00095                   * \param[in] functor_feedback Optional: a pointer to a user function can be set here to be called on each LM loop iteration (eg to refresh the current state and error, refresh a GUI, etc.)
00096                   *
00097                   * List of optional parameters by name in "extra_params":
00098                   *             - "verbose": (default=0) If !=0, produce verbose ouput.
00099                   *             - "max_iterations": (default=100) Maximum number of Lev-Marq. iterations.
00100                   *
00101                   * \note The following graph types are supported: mrpt::poses::CNetworkOfPoses2D, mrpt::poses::CNetworkOfPoses3D, mrpt::poses::CNetworkOfPoses2DInf, mrpt::poses::CNetworkOfPoses3DInf
00102                   *
00103                   * \tparam EDGE_TYPE The type of the edges. Typically users won't have to write this template argument by hand, since the compiler will auto-fit it depending on the type of the graph object.
00104                   * \tparam MAPS_IMPLEMENTATION The implementation for the map: NodeID -> node_pose. Read more on this in mrpt::poses::CNetworkOfPoses
00105                   * \sa The example "graph_slam_demo"
00106                   */
00107                 template <class EDGE_TYPE, class MAPS_IMPLEMENTATION>
00108                 void SLAM_IMPEXP optimize_graph_spa_levmarq(
00109                         mrpt::poses::CNetworkOfPoses<EDGE_TYPE,MAPS_IMPLEMENTATION > & graph,
00110                         TResultInfoSpaLevMarq                                        & out_info,
00111                         const std::set<mrpt::utils::TNodeID>            * nodes_to_optimize = NULL,
00112                         const mrpt::utils::TParametersDouble            & extra_params = mrpt::utils::TParametersDouble(),
00113                         typename graphslam_traits<EDGE_TYPE,MAPS_IMPLEMENTATION>::TFunctorFeedback  functor_feedback = NULL
00114                         );
00115 
00116 
00117 
00118 
00119         } // End of namespace
00120 } // End of namespace
00121 
00122 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011