00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 namespace mrpt 00038 { 00039 namespace slam 00040 { 00041 using namespace mrpt::poses; 00042 using namespace mrpt::math; 00043 00044 class CLandmarksMap; 00045 class CObservationBeaconRanges; 00046 00047 /** An implementation of rejection sampling for generating 2D robot pose from range-only measurements within a landmarks (beacons) map. 00048 * Before calling the method "rejectionSampling" to generate the samples, you must call "setParams". 00049 * It is assumed a planar scenario, where the robot is at a fixed height (default=0). 00050 * \sa bayes::CRejectionSamplingCapable 00051 */ 00052 class MRPTDLLIMPEXP CRejectionSamplingRangeOnlyLocalization : public bayes::CRejectionSamplingCapable<poses::CPose2D> 00053 { 00054 00055 public: 00056 /** Constructor 00057 */ 00058 CRejectionSamplingRangeOnlyLocalization(); 00059 00060 /** Destructor 00061 */ 00062 virtual ~CRejectionSamplingRangeOnlyLocalization() { } 00063 00064 /** The parameters used in the generation of random samples: 00065 * \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. 00066 * \param observation An observation with, at least ONE range measurement. 00067 * \param sigmaRanges The standard deviation of the "range measurement noise". 00068 * \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. 00069 * \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). 00070 * This method fills out the member "m_dataPerBeacon". 00071 * \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. 00072 */ 00073 bool setParams( 00074 const CLandmarksMap &beaconsMap, 00075 const CObservationBeaconRanges &observation, 00076 float sigmaRanges, 00077 const CPose2D &oldPose, 00078 float robot_z = 0, 00079 bool autoCheckAngleRanges = true); 00080 00081 protected: 00082 /** Generates one sample, drawing from some proposal distribution. 00083 */ 00084 void RS_drawFromProposal( CPose2D &outSample ); 00085 00086 /** Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the state space (values in the range [0,1]). 00087 */ 00088 double RS_observationLikelihood( const CPose2D &x); 00089 00090 /** Z coordinate of the robot. 00091 */ 00092 float m_z_robot; 00093 00094 float m_sigmaRanges; 00095 CPose2D m_oldPose; 00096 00097 /** The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood) 00098 */ 00099 size_t m_drawIndex; 00100 00101 /** Data for each beacon observation with a correspondence with the map. 00102 */ 00103 struct MRPTDLLIMPEXP TDataPerBeacon 00104 { 00105 TDataPerBeacon() : sensorOnRobot(), beaconPosition(), radiusAtRobotPlane(0),minAngle(0),maxAngle(0) 00106 {} 00107 00108 TPoint3D sensorOnRobot; 00109 TPoint2D beaconPosition; 00110 float radiusAtRobotPlane; 00111 float minAngle,maxAngle; 00112 }; 00113 00114 /** Data for each beacon observation with a correspondence with the map. 00115 */ 00116 std::deque<TDataPerBeacon> m_dataPerBeacon; 00117 00118 }; 00119 00120 } // End of namespace 00121 } // End of namespace 00122 00123 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:20:53 EDT 2009 |