Main MRPT website > C++ reference
MRPT logo

CIncrementalMapPartitioner.h

Go to the documentation of this file.
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               &section);
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.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011