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 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 * \sa CSimpleMap, CPosePDF, CObservation, utils::CDebugOutputCapable 00068 */ 00069 class SLAM_IMPEXP CConsistentObservationAlignment : public mrpt::utils::CDebugOutputCapable 00070 { 00071 protected: 00072 /** A sequence of probabilistic poses: 00073 */ 00074 typedef std::vector<CPosePDFGaussianPtr> vector_posesPdf; 00075 00076 public: 00077 00078 CConsistentObservationAlignment(); 00079 00080 /** The options for the method. 00081 */ 00082 struct SLAM_IMPEXP TOptions 00083 { 00084 /** Initialization: 00085 */ 00086 TOptions(); 00087 00088 /** If set to true (default), the matching will be performed against grid maps, instead of points maps: 00089 */ 00090 bool matchAgainstGridmap; 00091 00092 /** The resolution of the grid maps (default = 0.02m) 00093 */ 00094 float gridMapsResolution; 00095 00096 /** The options for building temporary maps. 00097 */ 00098 CPointsMap::TInsertionOptions pointsMapOptions; 00099 00100 /** The options for building temporary maps. 00101 */ 00102 COccupancyGridMap2D::TInsertionOptions gridInsertOptions; 00103 00104 /** The options for the ICP algorithm. 00105 */ 00106 CICP::TConfigParams icpOptions; 00107 00108 } options; 00109 00110 /** Executes the algorithm. See description in CConsistentObservationAlignment. 00111 * 00112 * \param inputMap The input to the algorithm: a set of nodes situated (with global coordinates) and observations from each node. 00113 * \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. 00114 */ 00115 void execute( 00116 CSimpleMap &inputMap, 00117 CSimpleMap &outputMap ); 00118 00119 /** 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. 00120 * \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). 00121 * \param out_OptimalPoses The 1xN vector with the consistent global poses of all nodes, where the first node is always at (0,0,0deg). 00122 */ 00123 static void optimizeUserSuppliedData( 00124 math::CMatrixTemplateObjects<CPosePDFGaussian> &in_PoseConstraints, 00125 math::CMatrixTemplateObjects<CPosePDFGaussian> &out_OptimalPoses ); 00126 00127 /** A textual description for the implemented algorithm. 00128 */ 00129 std::string getAlgorithmName(); 00130 00131 }; // End of class def. 00132 00133 } // End of namespace 00134 } // End of namespace 00135 00136 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
