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