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.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |