00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 00033 #include <mrpt/slam/link_pragmas.h> 00034 00035 namespace mrpt 00036 { 00037 namespace slam 00038 { 00039 /** An algorithm for optimizing a network of 2D or 3D pose links based on Levenberg-Marquardt error minimization. 00040 * It is computed the list of optimal, consistent global coordinates for each node in the graph. Relative poses 00041 * are represented by Gaussians with a mean and a covariance, which is also taken into account in the optimization. 00042 * 00043 * The algorithm can be seen as an extension to the work of Lu & Milios 00044 * - Globally Consistent Range Scan Alignment for Environment Mapping, 1997. 00045 * 00046 * \param pose_graph [IN] The graph of pose constraints. It can be of either type CNetworkOfPoses2D or CNetworkOfPoses3D. 00047 * \param optimal_poses [OUT] The consistent, global coordinates of all the pose nodes in the graph. 00048 * \param origin_pose [IN] Due to the degrees of freedom, one arbitrary pose is set to the origin (0,0,0). This parameter defines the pose ID of the pose to be taken as the origin, and the default value (ID:-1) will select the reference pose in the first link in pose_graph. 00049 * \param max_iterations [IN] The maximum number of iterations. If it is set to 0, the global poses computed as initial values from Dijkstra will be returned. 00050 * 00051 * \note Output covariances should be not considered, they are not computed yet. 00052 * \return The average square root error in the optimized pose network. 00053 * \sa optimizePoseGraph_stogradesc 00054 */ 00055 template <class CPOSE> 00056 double SLAM_IMPEXP optimizePoseGraph_levmarq( 00057 const CNetworkOfPoses<CPOSE> &pose_graph, 00058 std::map<size_t,CPOSE> &optimal_poses, 00059 const size_t max_iterations = 100, 00060 const size_t origin_pose = static_cast<size_t>(-1) 00061 ); 00062 00063 /** An algorithm for optimizing a network of 2D or 3D pose links based on stochastic gradient descent. 00064 * It is computed the list of optimal, consistent global coordinates for each node in the graph. Relative poses 00065 * are represented by Gaussians with a mean and a covariance, which is also taken into account in the optimization. 00066 * 00067 * This class is a C++ implementation of the work proposed in the paper: 00068 * - Edwin Olson, John Leonard, Seth Teller, "Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates", ICRA 2006. 00069 * 00070 * \param pose_graph [IN] The graph of pose constraints. It can be of either type CNetworkOfPoses2D or CNetworkOfPoses3D. 00071 * \param optimal_poses [OUT] The consistent, global coordinates of all the pose nodes in the graph. 00072 * \param origin_pose [IN] Due to the degrees of freedom, one arbitrary pose is set to the origin (0,0,0). This parameter defines the pose ID of the pose to be taken as the origin, and the default value (ID:-1) will select the reference pose in the first link in pose_graph. 00073 * \param max_iterations [IN] The maximum number of iterations. If it is set to 0, the global poses computed as initial values from Dijkstra will be returned. 00074 * 00075 * \return The average square root error in the optimized pose network. 00076 * \sa optimizePoseGraph_levmarq 00077 */ 00078 template <class CPOSE> 00079 double SLAM_IMPEXP optimizePoseGraph_stogradesc( 00080 const CNetworkOfPoses<CPOSE> &pose_graph, 00081 std::map<size_t,CPOSE> &optimal_poses, 00082 const size_t max_iterations = 100, 00083 const size_t origin_pose = static_cast<size_t>(-1) 00084 ); 00085 00086 } // End of namespace 00087 } // End of namespace 00088 00089 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
