Main MRPT website > C++ reference
MRPT logo
CRejectionSamplingRangeOnlyLocalization.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CRejectionSamplingRangeOnlyLocalization_H
29 #define CRejectionSamplingRangeOnlyLocalization_H
30 
31 #include <mrpt/poses/CPose2D.h>
32 #include <mrpt/poses/CPoint3D.h>
33 #include <mrpt/poses/CPoint2D.h>
34 #include <mrpt/bayes/CRejectionSamplingCapable.h>
35 #include <mrpt/math/lightweight_geom_data.h>
36 
37 #include <mrpt/slam/link_pragmas.h>
38 
39 namespace mrpt
40 {
41  namespace slam
42  {
43  using namespace mrpt::poses;
44  using namespace mrpt::math;
45 
46  class CLandmarksMap;
47  class CObservationBeaconRanges;
48 
49  /** An implementation of rejection sampling for generating 2D robot pose from range-only measurements within a landmarks (beacons) map.
50  * Before calling the method "rejectionSampling" to generate the samples, you must call "setParams".
51  * It is assumed a planar scenario, where the robot is at a fixed height (default=0).
52  * \sa bayes::CRejectionSamplingCapable \ingroup mrpt_slam_grp
53  */
54  class SLAM_IMPEXP CRejectionSamplingRangeOnlyLocalization : public bayes::CRejectionSamplingCapable<poses::CPose2D>
55  {
56 
57  public:
58  /** Constructor
59  */
60  CRejectionSamplingRangeOnlyLocalization();
61 
62  /** Destructor
63  */
64  virtual ~CRejectionSamplingRangeOnlyLocalization() { }
65 
66  /** The parameters used in the generation of random samples:
67  * \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.
68  * \param observation An observation with, at least ONE range measurement.
69  * \param sigmaRanges The standard deviation of the "range measurement noise".
70  * \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.
71  * \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).
72  * This method fills out the member "m_dataPerBeacon".
73  * \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.
74  */
75  bool setParams(
76  const CLandmarksMap &beaconsMap,
77  const CObservationBeaconRanges &observation,
78  float sigmaRanges,
79  const CPose2D &oldPose,
80  float robot_z = 0,
81  bool autoCheckAngleRanges = true);
82 
83  protected:
84  /** Generates one sample, drawing from some proposal distribution.
85  */
86  void RS_drawFromProposal( CPose2D &outSample );
87 
88  /** Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the state space (values in the range [0,1]).
89  */
90  double RS_observationLikelihood( const CPose2D &x);
91 
92  /** Z coordinate of the robot.
93  */
94  float m_z_robot;
95 
96  float m_sigmaRanges;
97  CPose2D m_oldPose;
98 
99  /** The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood)
100  */
101  size_t m_drawIndex;
102 
103  /** Data for each beacon observation with a correspondence with the map.
104  */
105  struct SLAM_IMPEXP TDataPerBeacon
106  {
107  TDataPerBeacon() : sensorOnRobot(), beaconPosition(), radiusAtRobotPlane(0),minAngle(0),maxAngle(0)
108  {}
109 
110  TPoint3D sensorOnRobot;
111  TPoint2D beaconPosition;
112  float radiusAtRobotPlane;
113  float minAngle,maxAngle;
114  };
115 
116  /** Data for each beacon observation with a correspondence with the map.
117  */
118  std::deque<TDataPerBeacon> m_dataPerBeacon;
119 
120  };
121 
122  } // End of namespace
123 } // End of namespace
124 
125 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013