00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 namespace mrpt 00045 { 00046 namespace poses { class CPose3DPDF; } 00047 namespace slam 00048 { 00049 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CIncrementalMapPartitioner, mrpt::utils::CSerializable, SLAM_IMPEXP ) 00050 00051 /** This class can be used to make partitions on a map/graph build from 00052 * observations taken at some poses/nodes. 00053 * \ingroup mrpt_slam_grp 00054 */ 00055 class SLAM_IMPEXP 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 SLAM_IMPEXP 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 template <class MATRIX> 00168 void getAdjacencyMatrix( MATRIX &outMatrix ) const { outMatrix = m_A; } 00169 00170 /** Returns a const ref to the internal adjacency matrix. */ 00171 const CMatrixDouble & getAdjacencyMatrix( ) const { return m_A; } 00172 00173 /** Read-only access to the sequence of Sensory Frames 00174 */ 00175 const CSimpleMap* getSequenceOfFrames( ) const 00176 { 00177 return &m_individualFrames; 00178 } 00179 00180 /** Access to the sequence of Sensory Frames 00181 */ 00182 CSimpleMap* getSequenceOfFrames( ) 00183 { 00184 return &m_individualFrames; 00185 } 00186 00187 /** Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering just those affected by aditions of new arcs. 00188 */ 00189 void markAllNodesForReconsideration(); 00190 00191 /** Change the coordinate origin of all stored poses, for consistency with future new poses to enter in the system. */ 00192 void changeCoordinatesOrigin( const CPose3D &newOrigin ); 00193 00194 /** 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. */ 00195 void changeCoordinatesOriginPoseIndex( const unsigned &newOriginPose ); 00196 00197 /** Returns a 3D representation of the current state: poses & links between them. 00198 * The previous contents of "objs" will be discarded 00199 */ 00200 void getAs3DScene( 00201 mrpt::opengl::CSetOfObjectsPtr &objs, 00202 const std::map< uint32_t, int64_t > *renameIndexes = NULL 00203 ) const; 00204 00205 private: 00206 /** El conjunto de los scans se guardan en un array: 00207 */ 00208 CSimpleMap m_individualFrames; 00209 std::deque<mrpt::slam::CMultiMetricMap> m_individualMaps; 00210 00211 00212 /** Adjacency matrix 00213 */ 00214 CMatrixD m_A; 00215 00216 /** The last partition 00217 */ 00218 std::vector<vector_uint> m_last_partition; 00219 00220 /** This will be true after adding new observations, and before an "updatePartitions" is invoked. 00221 */ 00222 bool m_last_last_partition_are_new_ones; 00223 00224 /** La lista de nodos que hay que tener en cuenta en la proxima actualizacion: 00225 */ 00226 std::vector<uint8_t> m_modified_nodes; 00227 00228 }; // End of class def. 00229 00230 00231 } // End of namespace 00232 } // End of namespace 00233 00234 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |