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 CCONSISTENTOBSERVATIONALIGNMENT_H 00029 #define CCONSISTENTOBSERVATIONALIGNMENT_H 00030 00031 #include <mrpt/slam/CSimpleMap.h> 00032 #include <mrpt/poses/CPose2D.h> 00033 #include <mrpt/slam/CPointsMap.h> 00034 #include <mrpt/slam/CICP.h> 00035 #include <mrpt/poses/CPosePDFGaussian.h> 00036 #include <mrpt/slam/COccupancyGridMap2D.h> 00037 00038 #include <mrpt/utils/CDebugOutputCapable.h> 00039 #include <mrpt/math/CMatrix.h> 00040 #include <mrpt/math/CMatrixTemplateObjects.h> 00041 00042 #include <mrpt/slam/link_pragmas.h> 00043 00044 namespace mrpt 00045 { 00046 namespace slam 00047 { 00048 using namespace mrpt::math; 00049 00050 /** An algorithm for globally, consistent alignment of a 00051 * sequence of observations. 00052 * This algorithm is based on the work of Lu & Milios 00053 * [Globally Consistent Range Scan Alignment for Environment Mapping, 1997] 00054 * for a global optimal estimation of laser range scan poses, but in 00055 * this case it has been extended to include any type of 00056 * observations as long as points-map-like operators are implemented over them. 00057 * <br> 00058 * <b>This class work in the following way:</b><br> 00059 * The input is a set of observations with associated "global" poses. This is 00060 * supplied with a "CSimpleMap" object, but the probabilistic poses 00061 * are ignored since only the mean values for the pose of each node are taken.<br> 00062 * After invoking the algorithm with CConsistentObservationAlignment::execute(), 00063 * a new "CSimpleMap" object is returned, where the 00064 * NOTE: The first node on the input map is used as reference and therefore 00065 * its pose is the only one which will never change. 00066 * 00067 * \note This class is superseded by more modern implementations of graph-slam. See mrpt::graphslam 00068 * 00069 * \sa CSimpleMap, CPosePDF, CObservation, utils::CDebugOutputCapable 00070 */ 00071 class SLAM_IMPEXP CConsistentObservationAlignment : public mrpt::utils::CDebugOutputCapable 00072 { 00073 protected: 00074 /** A sequence of probabilistic poses: 00075 */ 00076 typedef std::vector<CPosePDFGaussianPtr> vector_posesPdf; 00077 00078 public: 00079 00080 CConsistentObservationAlignment(); 00081 00082 /** The options for the method. 00083 */ 00084 struct SLAM_IMPEXP TOptions 00085 { 00086 /** Initialization: 00087 */ 00088 TOptions(); 00089 00090 /** If set to true (default), the matching will be performed against grid maps, instead of points maps: 00091 */ 00092 bool matchAgainstGridmap; 00093 00094 /** The resolution of the grid maps (default = 0.02m) 00095 */ 00096 float gridMapsResolution; 00097 00098 /** The options for building temporary maps. 00099 */ 00100 CPointsMap::TInsertionOptions pointsMapOptions; 00101 00102 /** The options for building temporary maps. 00103 */ 00104 COccupancyGridMap2D::TInsertionOptions gridInsertOptions; 00105 00106 /** The options for the ICP algorithm. 00107 */ 00108 CICP::TConfigParams icpOptions; 00109 00110 } options; 00111 00112 /** Executes the algorithm. See description in CConsistentObservationAlignment. 00113 * 00114 * \param inputMap The input to the algorithm: a set of nodes situated (with global coordinates) and observations from each node. 00115 * \param outputMap The globally consistent map, where probabilitic poses are filled with gaussian PDFs, where the mean is the globally optimal estimation and the covariance is also computed. 00116 */ 00117 void execute( 00118 CSimpleMap &inputMap, 00119 CSimpleMap &outputMap ); 00120 00121 /** This alternate method provides the basic consistent alignment algorithm to any user-supplied matrix of pose constrainsts, returning the optimal poses of all the nodes relative to the first one. 00122 * \param in_PoseConstraints This is a NxN matrix where element M(i,j) is the pose constrainst between node "i" and "j". Please, fill out only the upper-triangle part of the matrix (diagonal and lowe-part entries are not used). 00123 * \param out_OptimalPoses The 1xN vector with the consistent global poses of all nodes, where the first node is always at (0,0,0deg). 00124 */ 00125 static void optimizeUserSuppliedData( 00126 math::CMatrixTemplateObjects<CPosePDFGaussian> &in_PoseConstraints, 00127 math::CMatrixTemplateObjects<CPosePDFGaussian> &out_OptimalPoses ); 00128 00129 /** A textual description for the implemented algorithm. 00130 */ 00131 std::string getAlgorithmName(); 00132 00133 }; // End of class def. 00134 00135 } // End of namespace 00136 } // End of namespace 00137 00138 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011 |