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 #ifndef CBeaconMap_H 00029 #define CBeaconMap_H 00030 00031 #include <mrpt/slam/CMetricMap.h> 00032 #include <mrpt/slam/CBeacon.h> 00033 #include <mrpt/utils/CSerializable.h> 00034 #include <mrpt/math/CMatrix.h> 00035 #include <mrpt/utils/CDynamicGrid.h> 00036 #include <mrpt/utils/CLoadableOptions.h> 00037 00038 #include <mrpt/maps/link_pragmas.h> 00039 00040 namespace mrpt 00041 { 00042 namespace slam 00043 { 00044 using namespace mrpt::utils; 00045 using namespace mrpt::math; 00046 00047 class CObservationBeaconRanges; 00048 00049 00050 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CBeaconMap, CMetricMap ,MAPS_IMPEXP ) 00051 00052 /** A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM). 00053 * <br> 00054 * The individual beacons are defined as mrpt::slam::CBeacon objects. 00055 * <br> 00056 * When invoking CBeaconMap::insertObservation(), landmarks will be extracted and fused into the map. 00057 * The only currently supported observation type is mrpt::slam::CObservationBeaconRanges. 00058 * See insertionOptions and likelihoodOptions for parameters used when creating and fusing beacon landmarks. 00059 * <br> 00060 * Use "TInsertionOptions::insertAsMonteCarlo" to select between 2 different behaviors: 00061 * - Initial PDF of beacons: MonteCarlo, after convergence, pass to Gaussians; or 00062 * - Initial PDF of beacons: SOG, after convergence, a single Gaussian. 00063 * 00064 * Refer to the papers: [] 00065 * 00066 * \sa CMetricMap 00067 */ 00068 class MAPS_IMPEXP CBeaconMap : public CMetricMap 00069 { 00070 // This must be added to any CSerializable derived class: 00071 DEFINE_SERIALIZABLE( CBeaconMap ) 00072 00073 public: 00074 typedef std::deque<CBeacon> TSequenceBeacons; 00075 typedef std::deque<CBeacon>::iterator iterator; 00076 typedef std::deque<CBeacon>::const_iterator const_iterator; 00077 00078 protected: 00079 /** The individual beacons */ 00080 TSequenceBeacons m_beacons; 00081 00082 /** Clear the map, erasing all landmarks. 00083 */ 00084 virtual void internal_clear(); 00085 00086 /** Insert the observation information into this map. This method must be implemented 00087 * in derived classes. 00088 * \param obs The observation 00089 * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg) 00090 * 00091 * \sa CObservation::insertObservationInto 00092 */ 00093 virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL ); 00094 00095 public: 00096 /** Constructor */ 00097 CBeaconMap(); 00098 00099 void resize(const size_t N); //!< Resize the number of SOG modes 00100 00101 /** Access to individual beacons */ 00102 const CBeacon& operator [](size_t i) const { 00103 ASSERT_(i<m_beacons.size()) 00104 return m_beacons[i]; 00105 } 00106 /** Access to individual beacons */ 00107 const CBeacon& get(size_t i) const{ 00108 ASSERT_(i<m_beacons.size()) 00109 return m_beacons[i]; 00110 } 00111 /** Access to individual beacons */ 00112 CBeacon& operator [](size_t i) { 00113 ASSERT_(i<m_beacons.size()) 00114 return m_beacons[i]; 00115 } 00116 /** Access to individual beacons */ 00117 CBeacon& get(size_t i) { 00118 ASSERT_(i<m_beacons.size()) 00119 return m_beacons[i]; 00120 } 00121 00122 iterator begin() { return m_beacons.begin(); } 00123 const_iterator begin() const { return m_beacons.begin(); } 00124 iterator end() { return m_beacons.end(); } 00125 const_iterator end() const { return m_beacons.end(); } 00126 00127 /** Inserts a copy of the given mode into the SOG */ 00128 void push_back(const CBeacon& m) { 00129 m_beacons.push_back( m ); 00130 } 00131 00132 /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" 00133 * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps. 00134 * \param otherMap [IN] The other map to compute the matching with. 00135 * \param otherMapPose [IN] The 6D pose of the other map as seen from "this". 00136 * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. 00137 * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. 00138 * 00139 * \return The matching ratio [0,1] 00140 * \sa computeMatchingWith2D 00141 */ 00142 float compute3DMatchingRatio( 00143 const CMetricMap *otherMap, 00144 const CPose3D &otherMapPose, 00145 float minDistForCorr = 0.10f, 00146 float minMahaDistForCorr = 2.0f 00147 ) const; 00148 00149 /** With this struct options are provided to the likelihood computations. 00150 */ 00151 struct MAPS_IMPEXP TLikelihoodOptions : public utils::CLoadableOptions 00152 { 00153 public: 00154 /** Initilization of default parameters 00155 */ 00156 TLikelihoodOptions(); 00157 00158 /** See utils::CLoadableOptions 00159 */ 00160 void loadFromConfigFile( 00161 const mrpt::utils::CConfigFileBase &source, 00162 const std::string §ion); 00163 00164 /** See utils::CLoadableOptions 00165 */ 00166 void dumpToTextStream(CStream &out) const; 00167 00168 /** The standard deviation used for Beacon ranges likelihood (default=0.08m). 00169 */ 00170 float rangeStd; 00171 00172 } likelihoodOptions; 00173 00174 /** This struct contains data for choosing the method by which new beacons are inserted in the map. 00175 */ 00176 struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions 00177 { 00178 public: 00179 /** Initilization of default parameters 00180 */ 00181 TInsertionOptions(); 00182 00183 /** See utils::CLoadableOptions 00184 */ 00185 void loadFromConfigFile( 00186 const mrpt::utils::CConfigFileBase &source, 00187 const std::string §ion); 00188 00189 /** See utils::CLoadableOptions 00190 */ 00191 void dumpToTextStream(CStream &out) const; 00192 00193 /** Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussians (see mrpt::slam::CBeacon). 00194 * \sa MC_performResampling 00195 */ 00196 bool insertAsMonteCarlo; 00197 00198 /** Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation: the default values (both 0), makes the beacons to be in the same horizontal plane that the sensors, that is, 2D SLAM - the min/max values are -90/90. 00199 */ 00200 float maxElevation_deg,minElevation_deg; 00201 00202 /** Number of particles per meter of range, i.e. per meter of the "radius of the ring". 00203 */ 00204 unsigned int MC_numSamplesPerMeter; 00205 00206 /** The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (default=0,4). 00207 */ 00208 float MC_maxStdToGauss; 00209 00210 /** Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing a given sample (default=5). 00211 */ 00212 float MC_thresholdNegligible; 00213 00214 /** If set to false (default), the samples will be generated the first time a beacon is observed, and their weights just updated subsequently - if set to "true", fewer samples will be required since the particles will be resamples when necessary, and a small "noise" will be added to avoid depletion. 00215 */ 00216 bool MC_performResampling; 00217 00218 /** The std.dev. of the Gaussian noise to be added to each sample after resampling, only if MC_performResampling=true. 00219 */ 00220 float MC_afterResamplingNoise; 00221 00222 /** Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mode (default=20). 00223 */ 00224 float SOG_thresholdNegligible; 00225 00226 /** A parameter for initializing 2D/3D SOGs 00227 */ 00228 float SOG_maxDistBetweenGaussians; 00229 00230 /** Constant used to compute the std. dev. int the tangent direction when creating the Gaussians. 00231 */ 00232 float SOG_separationConstant; 00233 00234 } insertionOptions; 00235 00236 /** Save to a MATLAB script which displays 3D error ellipses for the map. 00237 * \param file The name of the file to save the script to. 00238 * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities) 00239 * \param stdCount The ellipsoids will be drawn from the center to a given confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95 confidence intervals) 00240 * 00241 * \return Returns false if any error occured, true elsewere. 00242 */ 00243 bool saveToMATLABScript3D( 00244 std::string file, 00245 const char *style="b", 00246 float confInterval = 0.95f ) const; 00247 00248 00249 /** Returns the stored landmarks count. 00250 */ 00251 size_t size() const; 00252 00253 00254 /** Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the world being modeled with this map. 00255 * 00256 * In the current implementation, this method behaves in a different way according to the nature of 00257 * the observation's class: 00258 * - "mrpt::slam::CObservation2DRangeScan": This calls "computeLikelihood_RSLC_2007". 00259 * - "mrpt::slam::CObservationStereoImages": This calls "computeLikelihood_SIFT_LandmarkMap". 00260 * 00261 * \param takenFrom The robot's pose the observation is supposed to be taken from. 00262 * \param obs The observation. 00263 * \return This method returns a likelihood value > 0. 00264 * 00265 * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update 00266 */ 00267 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ); 00268 00269 /** Computes the matchings between this and another 2D points map. 00270 This includes finding: 00271 - The set of points pairs in each map 00272 - The mean squared distance between corresponding pairs. 00273 This method is the most time critical one into the ICP algorithm. 00274 00275 * \param otherMap [IN] The other map to compute the matching with. 00276 * \param otherMapPose [IN] The pose of the other map as seen from "this". 00277 * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched. 00278 * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences. 00279 * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps. 00280 * \param correspondences [OUT] The detected matchings pairs. 00281 * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence. 00282 * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default. 00283 * \param covariance [OUT] The resulting matching covariance 3x3 matrix, or NULL if undesired. 00284 * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned. 00285 * 00286 * \sa compute3DMatchingRatio 00287 */ 00288 void computeMatchingWith2D( 00289 const CMetricMap *otherMap, 00290 const CPose2D &otherMapPose, 00291 float maxDistForCorrespondence, 00292 float maxAngularDistForCorrespondence, 00293 const CPose2D &angularDistPivotPoint, 00294 TMatchingPairList &correspondences, 00295 float &correspondencesRatio, 00296 float *sumSqrDist = NULL, 00297 bool onlyKeepTheClosest = false, 00298 bool onlyUniqueRobust = false ) const; 00299 00300 /** Perform a search for correspondences between "this" and another lansmarks map: 00301 * Firsly, the landmarks' descriptor is used to find correspondences, then inconsistent ones removed by 00302 * looking at their 3D poses. 00303 * \param otherMap [IN] The other map. 00304 * \param correspondences [OUT] The matched pairs between maps. 00305 * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap 00306 * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence. 00307 */ 00308 void computeMatchingWith3DLandmarks( 00309 const mrpt::slam::CBeaconMap *otherMap, 00310 TMatchingPairList &correspondences, 00311 float &correspondencesRatio, 00312 std::vector<bool> &otherCorrespondences) const; 00313 00314 /** Changes the reference system of the map to a given 3D pose. 00315 */ 00316 void changeCoordinatesReference( const CPose3D &newOrg ); 00317 00318 /** Changes the reference system of the map "otherMap" and save the result in "this" map. 00319 */ 00320 void changeCoordinatesReference( const CPose3D &newOrg, const mrpt::slam::CBeaconMap *otherMap ); 00321 00322 00323 /** Returns true if the map is empty/no observation has been inserted. 00324 */ 00325 bool isEmpty() const; 00326 00327 /** Simulates a reading toward each of the beacons in the landmarks map, if any. 00328 * \param in_robotPose This robot pose is used to simulate the ranges to each beacon. 00329 * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot 00330 * \param out_Observations The results will be stored here. NOTICE that the fields "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance" and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before calling this function. 00331 * An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range. 00332 */ 00333 void simulateBeaconReadings( 00334 const CPose3D &in_robotPose, 00335 const CPoint3D &in_sensorLocationOnRobot, 00336 CObservationBeaconRanges &out_Observations ) const; 00337 00338 /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface). 00339 * In the case of this class, these files are generated: 00340 * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses. 00341 * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D. 00342 * - "filNamePrefix"+"_covs.m": A textual representation (see saveToTextFile) 00343 */ 00344 void saveMetricMapRepresentationToFile( 00345 const std::string &filNamePrefix ) const; 00346 00347 /** Save a text file with a row per beacon, containing this 11 elements: 00348 * - X Y Z: Mean values 00349 * - VX VY VZ: Variances of each dimension (C11, C22, C33) 00350 * - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes. 00351 * - C12, C13, C23: Cross covariances 00352 */ 00353 void saveToTextFile(const std::string &fil) const; 00354 00355 /** Returns a 3D object representing the map. 00356 */ 00357 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00358 00359 /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". 00360 * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables. 00361 */ 00362 void auxParticleFilterCleanUp(); 00363 00364 /** Returns a pointer to the beacon with the given ID, or NULL if it does not exist. 00365 */ 00366 const CBeacon * getBeaconByID( CBeacon::TBeaconID id ) const; 00367 00368 /** Returns a pointer to the beacon with the given ID, or NULL if it does not exist. 00369 */ 00370 CBeacon * getBeaconByID( CBeacon::TBeaconID id ); 00371 00372 }; // End of class def. 00373 00374 00375 } // End of namespace 00376 } // End of namespace 00377 00378 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011 |