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 00029 #ifndef CINCREMENTALMAPPARTITIONER_H 00030 #define CINCREMENTALMAPPARTITIONER_H 00031 00032 #include <mrpt/utils/CDebugOutputCapable.h> 00033 #include <mrpt/utils/CLoadableOptions.h> 00034 00035 #include <mrpt/utils/stl_extensions.h> 00036 00037 #include <mrpt/slam/CMultiMetricMap.h> 00038 #include <mrpt/slam/CSimplePointsMap.h> 00039 #include <mrpt/slam/CSensFrameProbSequence.h> 00040 #include <mrpt/slam/CMultiMetricMap.h> 00041 00042 #include <map> 00043 00044 namespace mrpt 00045 { 00046 namespace poses { class CPose3DPDF; } 00047 namespace slam 00048 { 00049 00050 DEFINE_SERIALIZABLE_PRE( CIncrementalMapPartitioner ) 00051 00052 /** This class can be used to make partitions on a map/graph build from 00053 * observations taken at some poses/nodes. 00054 */ 00055 class MRPTDLLIMPEXP CIncrementalMapPartitioner : public utils::CDebugOutputCapable, public CSerializable 00056 { 00057 // This must be added to any CSerializable derived class: 00058 DEFINE_SERIALIZABLE( CIncrementalMapPartitioner ) 00059 00060 public: 00061 /** Constructor: 00062 */ 00063 CIncrementalMapPartitioner( ); 00064 00065 /** Destructor: 00066 */ 00067 virtual ~CIncrementalMapPartitioner(); 00068 00069 /** Initialization: Start of a new map, new internal matrices,... 00070 */ 00071 void clear(); 00072 00073 /** Configuration of the algorithm: 00074 */ 00075 struct MRPTDLLIMPEXP TOptions : public utils::CLoadableOptions 00076 { 00077 /** Sets default values at object creation 00078 */ 00079 TOptions(); 00080 00081 /** Load parameters from configuration source 00082 */ 00083 void loadFromConfigFile( 00084 const mrpt::utils::CConfigFileBase &source, 00085 const std::string §ion); 00086 00087 /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream. 00088 */ 00089 void dumpToTextStream(CStream &out) const; 00090 00091 /** The partition threshold for bisection in range [0,2], default=1.0 00092 */ 00093 float partitionThreshold; 00094 00095 /** For the occupancy grid maps of each node, default=0.10 00096 */ 00097 float gridResolution; 00098 00099 /** Used in the computation of weights, default=0.20 00100 */ 00101 float minDistForCorrespondence; 00102 00103 /** Used in the computation of weights, default=2.0 00104 */ 00105 float minMahaDistForCorrespondence; 00106 00107 /** If set to true (default), 1 or 2 clusters will be returned. Default=false -> Autodetermine the number of partitions. 00108 */ 00109 bool forceBisectionOnly; 00110 00111 /** If set to true (default), adjacency matrix is computed from maps matching; otherwise, the method CObservation::likelihoodWith will be called directly from the SFs. 00112 */ 00113 bool useMapMatching; 00114 00115 /** This variable will be mapped to utils::CGraphPartitioner::DEBUG_SAVE_EIGENVECTOR_FILES. 00116 */ 00117 bool debugSaveAllEigenvectors; 00118 00119 /** If a partition leads to a cluster with less elements than this, it will be rejected even if had a good Ncut (default=1). */ 00120 int minimumNumberElementsEachCluster; 00121 00122 } options; 00123 00124 /** Add a new frame to the current graph: call this method each time a new observation 00125 * is added to the map/graph, and whenever you want to update the partitions, call "updatePartitions" 00126 * \param frame The sensed data 00127 * \param robotPose An estimation of the robot global 2D pose. 00128 * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future. 00129 * \sa updatePartitions 00130 */ 00131 unsigned int addMapFrame( const CSensoryFramePtr &frame, const CPosePDFPtr &robotPose2D ); 00132 00133 /** Add a new frame to the current graph: call this method each time a new observation 00134 * is added to the map/graph, and whenever you want to update the partitions, call "updatePartitions" 00135 * \param frame The sensed data 00136 * \param robotPose An estimation of the robot global 2D pose. 00137 * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future. 00138 * \sa updatePartitions 00139 */ 00140 unsigned int addMapFrame( const CSensoryFramePtr &frame, const CPose3DPDFPtr &robotPose3D ); 00141 00142 /** Add a new frame to the current graph: call this method each time a new observation 00143 * is added to the map/graph, and whenever you want to update the partitions, call "updatePartitions" 00144 * \param frame The sensed data 00145 * \param robotPose An estimation of the robot global 2D pose. 00146 * \return The index of the new pose in the internal list, which will be used to refer to the pose in the future. 00147 * \sa updatePartitions 00148 */ 00149 unsigned int addMapFrame( const CSensoryFrame &frame, const CPose3DPDF &robotPose3D ); 00150 00151 /** This method executed only the neccesary part of the partition to take 00152 * into account the lastest added frames. 00153 * \sa addMapFrame 00154 */ 00155 void updatePartitions( std::vector<vector_uint> &partitions ); 00156 00157 /** It returns the nodes count currently in the internal map/graph. 00158 */ 00159 unsigned int getNodesCount(); 00160 00161 /** Remove the stated nodes (0-based indexes) from the internal lists. 00162 * If changeCoordsRef is true, coordinates are changed to leave the first node at (0,0,0). 00163 */ 00164 void removeSetOfNodes(vector_uint indexesToRemove, bool changeCoordsRef = true); 00165 00166 /** Returns a copy of the internal adjacency matrix. 00167 */ 00168 void getAdjacencyMatrix( CMatrix &outMatrix ) const; 00169 00170 /** Read-only access to the sequence of Sensory Frames 00171 */ 00172 const CSensFrameProbSequence* getSequenceOfFrames( ) const 00173 { 00174 return &m_individualFrames; 00175 } 00176 00177 /** Access to the sequence of Sensory Frames 00178 */ 00179 CSensFrameProbSequence* getSequenceOfFrames( ) 00180 { 00181 return &m_individualFrames; 00182 } 00183 00184 /** Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering just those affected by aditions of new arcs. 00185 */ 00186 void markAllNodesForReconsideration(); 00187 00188 /** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system. */ 00189 void changeCoordinatesOrigin( const CPose3D &newOrigin ); 00190 00191 /** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system; the new origin is given by the index of the pose to become the new origin. */ 00192 void changeCoordinatesOriginPoseIndex( const unsigned &newOriginPose ); 00193 00194 /** Returns a 3D representation of the current state: poses & links between them. 00195 * The previous contents of "objs" will be discarded 00196 */ 00197 void getAs3DScene( 00198 mrpt::opengl::CSetOfObjectsPtr &objs, 00199 const std::map< uint32_t, int64_t > *renameIndexes = NULL 00200 ) const; 00201 00202 private: 00203 /** El conjunto de los scans se guardan en un array: 00204 */ 00205 CSensFrameProbSequence m_individualFrames; 00206 std::deque<mrpt::slam::CMultiMetricMap> m_individualMaps; 00207 00208 00209 /** Adjacency matrix 00210 */ 00211 CMatrixD m_A; 00212 00213 /** The last partition 00214 */ 00215 std::vector<vector_uint> m_last_partition; 00216 00217 /** This will be true after adding new observations, and before an "updatePartitions" is invoked. 00218 */ 00219 bool m_last_last_partition_are_new_ones; 00220 00221 /** La lista de nodos que hay que tener en cuenta en la proxima actualizacion: 00222 */ 00223 std::vector<uint8_t> m_modified_nodes; 00224 00225 }; // End of class def. 00226 00227 00228 } // End of namespace 00229 } // End of namespace 00230 00231 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |