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 CONSTRAINED_POSE_NETWORK_H 00029 #define CONSTRAINED_POSE_NETWORK_H 00030 00031 #include <mrpt/poses/CPosePDFGaussian.h> 00032 #include <mrpt/poses/CPose3DPDFGaussian.h> 00033 #include <mrpt/math/graphs.h> 00034 00035 namespace mrpt 00036 { 00037 namespace poses 00038 { 00039 /** A network of links constraining the relative pose of pairs of nodes, indentified by their numeric IDs (of type size_t). 00040 * A link between nodes "i" and "j", that is, the pose \f$ p_{ij} \f$ or relative position of "j" with respect to "i", 00041 * is maintained as a multivariate Gaussian distribution. 00042 * 00043 * Valid values for the argument CPOSE are CPosePDFGaussian and CPose3DPDFGaussian, which correspond to the 00044 * typedefs CNetworkOfPoses2D and CNetworkOfPoses3D. 00045 * 00046 * Access to all the links can be done through normal std::map methods, plus the new method "insertLink" 00047 * 00048 * \sa CPosePDFGaussian,CPose3DPDFGaussian, mrpt::slam::optimizePoseGraph_levmarq 00049 */ 00050 template<class CPOSE> 00051 class MRPTDLLIMPEXP CNetworkOfPoses : public mrpt::math::CDirectedGraph< CPOSE > 00052 { 00053 public: 00054 typedef CPOSE type_poses; //!< The type of PDF poses on the network links 00055 typedef std::map<size_t,CPOSE> type_global_poses; //!< A map of pose IDs to their global coordinates - can be used as second parameter of optimizePoseGraph_levmarq 00056 }; 00057 00058 typedef CNetworkOfPoses<CPosePDFGaussian> CNetworkOfPoses2D; //!< The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian 00059 typedef CNetworkOfPoses<CPose3DPDFGaussian> CNetworkOfPoses3D; //!< The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian 00060 00061 /** An algorithm for optimizing a network of 2D or 3D pose links based on Levenberg-Marquardt error minimization. 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 * The algorithm can be seen as an extension to the work of Lu & Milios 00066 * - Globally Consistent Range Scan Alignment for Environment Mapping, 1997. 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 * \note Output covariances should be not considered, they are not computed yet. 00074 * \return The average square root error in the optimized pose network. 00075 * \sa optimizePoseGraph_stogradesc 00076 */ 00077 template <class CPOSE> 00078 double MRPTDLLIMPEXP optimizePoseGraph_levmarq( 00079 const CNetworkOfPoses<CPOSE> &pose_graph, 00080 std::map<size_t,CPOSE> &optimal_poses, 00081 const size_t max_iterations = 100, 00082 const size_t origin_pose = static_cast<size_t>(-1) 00083 ); 00084 00085 /** An algorithm for optimizing a network of 2D or 3D pose links based on stochastic gradient descent. 00086 * It is computed the list of optimal, consistent global coordinates for each node in the graph. Relative poses 00087 * are represented by Gaussians with a mean and a covariance, which is also taken into account in the optimization. 00088 * 00089 * This class is a C++ implementation of the work proposed in the paper: 00090 * - Edwin Olson, John Leonard, Seth Teller, "Fast Iterative Optimization of Pose Graphs with Poor Initial Estimates", ICRA 2006. 00091 * 00092 * \param pose_graph [IN] The graph of pose constraints. It can be of either type CNetworkOfPoses2D or CNetworkOfPoses3D. 00093 * \param optimal_poses [OUT] The consistent, global coordinates of all the pose nodes in the graph. 00094 * \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. 00095 * \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. 00096 * 00097 * \return The average square root error in the optimized pose network. 00098 * \sa optimizePoseGraph_levmarq 00099 */ 00100 template <class CPOSE> 00101 double MRPTDLLIMPEXP optimizePoseGraph_stogradesc( 00102 const CNetworkOfPoses<CPOSE> &pose_graph, 00103 std::map<size_t,CPOSE> &optimal_poses, 00104 const size_t max_iterations = 100, 00105 const size_t origin_pose = static_cast<size_t>(-1) 00106 ); 00107 00108 } // End of namespace 00109 } // End of namespace 00110 00111 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009 |