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:exported at Tue Jan 25 21:56:31 UTC 2011 |