MRPT  2.0.4
CRejectionSamplingRangeOnlyLocalization.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #include <mrpt/obs/obs_frwds.h>
13 #include <mrpt/poses/CPoint2D.h>
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose2D.h>
16 
17 namespace mrpt
18 {
19 namespace maps
20 {
21 class CLandmarksMap;
22 }
23 
24 namespace slam
25 {
26 /** An implementation of rejection sampling for generating 2D robot pose from
27  * range-only measurements within a landmarks (beacons) map.
28  * Before calling the method "rejectionSampling" to generate the samples, you
29  * must call "setParams".
30  * It is assumed a planar scenario, where the robot is at a fixed height
31  * (default=0).
32  * \sa bayes::CRejectionSamplingCapable \ingroup mrpt_slam_grp
33  */
35  : public bayes::CRejectionSamplingCapable<mrpt::poses::CPose2D>
36 {
37  public:
38  /** Constructor
39  */
41 
42  /** Destructor
43  */
44  ~CRejectionSamplingRangeOnlyLocalization() override = default;
45  /** The parameters used in the generation of random samples:
46  * \param beaconsMap The map containing the N beacons (indexed by their
47  * "beacon ID"s). Only the mean 3D position of the beacons is used, the
48  * covariance is ignored.
49  * \param observation An observation with, at least ONE range measurement.
50  * \param sigmaRanges The standard deviation of the "range measurement
51  * noise".
52  * \param robot_z The height of the robot on the floor (default=0). Note
53  * that the beacon sensor on the robot may be at a different height,
54  * according to data within the observation object.
55  * \param autoCheckAngleRanges Whether to make a simple check for potential
56  * good angles from the beacons to generate samples (disable to speed-up the
57  * preparation vs. making slower the drawn).
58  * This method fills out the member "m_dataPerBeacon".
59  * \return true if at least ONE beacon has been successfully loaded, false
60  * otherwise. In this case do not call "rejectionSampling" or an exception
61  * will be launch, since there is no information to generate samples.
62  */
63  bool setParams(
64  const mrpt::maps::CLandmarksMap& beaconsMap,
65  const mrpt::obs::CObservationBeaconRanges& observation,
66  float sigmaRanges, const mrpt::poses::CPose2D& oldPose,
67  float robot_z = 0, bool autoCheckAngleRanges = true);
68 
69  protected:
70  /** Generates one sample, drawing from some proposal distribution.
71  */
72  void RS_drawFromProposal(mrpt::poses::CPose2D& outSample) override;
73 
74  /** Returns the NORMALIZED observation likelihood (linear, not
75  * exponential!!!) at a given point of the state space (values in the range
76  * [0,1]).
77  */
78  double RS_observationLikelihood(const mrpt::poses::CPose2D& x) override;
79 
80  /** Z coordinate of the robot.
81  */
82  float m_z_robot{0};
83 
84  float m_sigmaRanges{0.10f};
86 
87  /** The index in "m_dataPerBeacon" used to draw samples (the rest will be
88  * used to evaluate the likelihood)
89  */
90  size_t m_drawIndex{0};
91 
92  /** Data for each beacon observation with a correspondence with the map.
93  */
95  {
96  TDataPerBeacon() = default;
97 
101  float minAngle{0}, maxAngle{0};
102  };
103 
104  /** Data for each beacon observation with a correspondence with the map.
105  */
106  std::deque<TDataPerBeacon> m_dataPerBeacon;
107 };
108 
109 } // namespace slam
110 } // namespace mrpt
void RS_drawFromProposal(mrpt::poses::CPose2D &outSample) override
Generates one sample, drawing from some proposal distribution.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Data for each beacon observation with a correspondence with the map.
size_t m_drawIndex
The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood...
bool setParams(const mrpt::maps::CLandmarksMap &beaconsMap, const mrpt::obs::CObservationBeaconRanges &observation, float sigmaRanges, const mrpt::poses::CPose2D &oldPose, float robot_z=0, bool autoCheckAngleRanges=true)
The parameters used in the generation of random samples:
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
std::deque< TDataPerBeacon > m_dataPerBeacon
Data for each beacon observation with a correspondence with the map.
double RS_observationLikelihood(const mrpt::poses::CPose2D &x) override
Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the st...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
~CRejectionSamplingRangeOnlyLocalization() override=default
Destructor.
A base class for implementing rejection sampling in a generic state space.
An implementation of rejection sampling for generating 2D robot pose from range-only measurements wit...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020