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 CCONSISTENTOBSERVATIONALIGNMENT_H 00029 #define CCONSISTENTOBSERVATIONALIGNMENT_H 00030 00031 #include <mrpt/slam/CSensFrameProbSequence.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 namespace mrpt 00043 { 00044 namespace slam 00045 { 00046 using namespace mrpt::math; 00047 00048 /** An algorithm for globally, consistent alignment of a 00049 * sequence of observations. 00050 * This algorithm is based on the work of Lu & Milios 00051 * [Globally Consistent Range Scan Alignment for Environment Mapping, 1997] 00052 * for a global optimal estimation of laser range scan poses, but in 00053 * this case it has been extended to include any type of 00054 * observations as long as points-map-like operators are implemented over them. 00055 <br> 00056 <b>This class work in the following way:</b><br> 00057 The input is a set of observations with associated "global" poses. This is 00058 supplied with a "CSensFrameProbSequence" object, but the probabilistic poses 00059 are ignored since only the mean values for the pose of each node are taken.<br> 00060 After invoking the algorithm with CConsistentObservationAlignment::execute(), 00061 a new "CSensFrameProbSequence" object is returned, where the 00062 NOTE: The first node on the input map is used as reference and therefore 00063 its pose is the only one which will never change. 00064 00065 * \sa CSensFrameProbSequence, CPosePDF, CObservation, utils::CDebugOutputCapable 00066 */ 00067 class MRPTDLLIMPEXP CConsistentObservationAlignment : public mrpt::utils::CDebugOutputCapable 00068 { 00069 protected: 00070 /** A sequence of probabilistic poses: 00071 */ 00072 typedef std::vector<CPosePDFGaussianPtr> vector_posesPdf; 00073 00074 public: 00075 00076 CConsistentObservationAlignment(); 00077 00078 /** The options for the method. 00079 */ 00080 struct MRPTDLLIMPEXP TOptions 00081 { 00082 /** Initialization: 00083 */ 00084 TOptions(); 00085 00086 /** If set to true (default), the matching will be performed against grid maps, instead of points maps: 00087 */ 00088 bool matchAgainstGridmap; 00089 00090 /** The resolution of the grid maps (default = 0.02m) 00091 */ 00092 float gridMapsResolution; 00093 00094 /** The options for building temporal maps. 00095 */ 00096 CPointsMap::TInsertionOptions pointsMapOptions; 00097 00098 /** The options for building temporal maps. 00099 */ 00100 COccupancyGridMap2D::TInsertionOptions gridInsertOptions; 00101 00102 /** The options for the ICP algorithm. 00103 */ 00104 CICP::TConfigParams icpOptions; 00105 00106 } options; 00107 00108 /** Executes the algorithm. See description in CConsistentObservationAlignment. 00109 * 00110 * \param inputMap The input to the algorithm: a set of nodes situated (with global coordinates) and observations from each node. 00111 * \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. 00112 */ 00113 void execute( 00114 CSensFrameProbSequence &inputMap, 00115 CSensFrameProbSequence &outputMap ); 00116 00117 /** 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. 00118 * \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). 00119 * \param out_OptimalPoses The 1xN vector with the consistent global poses of all nodes, where the first node is always at (0,0,0deg). 00120 */ 00121 static void optimizeUserSuppliedData( 00122 math::CMatrixTemplateObjects<CPosePDFGaussian> &in_PoseConstraints, 00123 math::CMatrixTemplateObjects<CPosePDFGaussian> &out_OptimalPoses ); 00124 00125 /** A textual description for the implemented algorithm. 00126 */ 00127 std::string getAlgorithmName(); 00128 00129 }; // End of class def. 00130 00131 } // End of namespace 00132 } // End of namespace 00133 00134 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |