Main MRPT website > C++ reference
MRPT logo

CRejectionSamplingRangeOnlyLocalization.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 #ifndef CRejectionSamplingRangeOnlyLocalization_H
00029 #define CRejectionSamplingRangeOnlyLocalization_H
00030 
00031 #include <mrpt/poses/CPose2D.h>
00032 #include <mrpt/poses/CPoint3D.h>
00033 #include <mrpt/poses/CPoint2D.h>
00034 #include <mrpt/bayes/CRejectionSamplingCapable.h>
00035 #include <mrpt/math/lightweight_geom_data.h>
00036 
00037 #include <mrpt/slam/link_pragmas.h>
00038 
00039 namespace mrpt
00040 {
00041         namespace slam
00042         {
00043                 using namespace mrpt::poses;
00044                 using namespace mrpt::math;
00045 
00046                 class   CLandmarksMap;
00047                 class   CObservationBeaconRanges;
00048 
00049                 /** An implementation of rejection sampling for generating 2D robot pose from range-only measurements within a landmarks (beacons) map.
00050                  *    Before calling the method "rejectionSampling" to generate the samples, you must call "setParams".
00051                  *    It is assumed a planar scenario, where the robot is at a fixed height (default=0).
00052                  * \sa bayes::CRejectionSamplingCapable
00053                  */
00054                 class SLAM_IMPEXP CRejectionSamplingRangeOnlyLocalization : public bayes::CRejectionSamplingCapable<poses::CPose2D>
00055                 {
00056 
00057                 public:
00058                         /** Constructor
00059                           */
00060                         CRejectionSamplingRangeOnlyLocalization();
00061 
00062                         /** Destructor
00063                           */
00064                         virtual ~CRejectionSamplingRangeOnlyLocalization() { }
00065 
00066                         /** The parameters used in the generation of random samples:
00067                           * \param beaconsMap The map containing the N beacons (indexed by their "beacon ID"s). Only the mean 3D position of the beacons is used, the covariance is ignored.
00068                           * \param observation An observation with, at least ONE range measurement.
00069                           * \param sigmaRanges The standard deviation of the "range measurement noise".
00070                           * \param robot_z The height of the robot on the floor (default=0). Note that the beacon sensor on the robot may be at a different height, according to data within the observation object.
00071                           * \param autoCheckAngleRanges Whether to make a simple check for potential good angles from the beacons to generate samples (disable to speed-up the preparation vs. making slower the drawn).
00072                           *  This method fills out the member "m_dataPerBeacon".
00073                           * \return true if at least ONE beacon has been successfully loaded, false otherwise. In this case do not call "rejectionSampling" or an exception will be launch, since there is no information to generate samples.
00074                           */
00075                         bool setParams(
00076                                 const CLandmarksMap                             &beaconsMap,
00077                                 const CObservationBeaconRanges  &observation,
00078                                 float                                                   sigmaRanges,
00079                                 const CPose2D                                   &oldPose,
00080                                 float                                                   robot_z = 0,
00081                                 bool                                                    autoCheckAngleRanges = true);
00082 
00083                 protected:
00084                         /** Generates one sample, drawing from some proposal distribution.
00085                           */
00086                         void RS_drawFromProposal( CPose2D &outSample );
00087 
00088                         /** Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the state space (values in the range [0,1]).
00089                           */
00090                         double RS_observationLikelihood( const CPose2D &x);
00091 
00092                         /** Z coordinate of the robot.
00093                           */
00094                         float           m_z_robot;
00095 
00096                         float           m_sigmaRanges;
00097                         CPose2D         m_oldPose;
00098 
00099                         /** The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood)
00100                           */
00101                         size_t          m_drawIndex;
00102 
00103                         /** Data for each beacon observation with a correspondence with the map.
00104                           */
00105                         struct SLAM_IMPEXP TDataPerBeacon
00106                         {
00107                                 TDataPerBeacon()  : sensorOnRobot(), beaconPosition(), radiusAtRobotPlane(0),minAngle(0),maxAngle(0)
00108                                 {}
00109 
00110                                 TPoint3D        sensorOnRobot;
00111                                 TPoint2D        beaconPosition;
00112                                 float           radiusAtRobotPlane;
00113                                 float           minAngle,maxAngle;
00114                         };
00115 
00116                         /** Data for each beacon observation with a correspondence with the map.
00117                           */
00118                         std::deque<TDataPerBeacon>      m_dataPerBeacon;
00119 
00120                 };
00121 
00122         } // End of namespace
00123 } // End of namespace
00124 
00125 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011