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