MRPT  2.0.4
CRejectionSamplingRangeOnlyLocalization.cpp
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 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/maps/CLandmark.h>
14 #include <mrpt/math/TPose2D.h>
15 #include <mrpt/math/utils.h>
18 
19 using namespace mrpt::math;
20 using namespace mrpt::slam;
21 using namespace mrpt::maps;
22 using namespace mrpt::obs;
23 using namespace mrpt::random;
24 using namespace mrpt::poses;
25 using namespace std;
26 
27 /*---------------------------------------------------------------
28  Constructor
29 ---------------------------------------------------------------*/
30 CRejectionSamplingRangeOnlyLocalization::
31  CRejectionSamplingRangeOnlyLocalization()
32  : m_oldPose(),
33 
34  m_dataPerBeacon()
35 {
36 }
37 
38 /*---------------------------------------------------------------
39  RS_drawFromProposal
40 ---------------------------------------------------------------*/
42  CPose2D& outSample)
43 {
45 
46  // Use the first beacon data to draw a pose:
47  if (m_dataPerBeacon.size() == 0)
49  "There is no information from which to draw samples!! Use "
50  "'setParams' with valid data!");
51 
53 
54  float ang = getRandomGenerator().drawUniform(
55  m_dataPerBeacon[m_drawIndex].minAngle,
56  m_dataPerBeacon[m_drawIndex].maxAngle);
58  m_dataPerBeacon[m_drawIndex].radiusAtRobotPlane, m_sigmaRanges);
59 
60  // This is the point where the SENSOR is:
61  outSample.x(m_dataPerBeacon[m_drawIndex].beaconPosition.x + cos(ang) * R);
62  outSample.y(m_dataPerBeacon[m_drawIndex].beaconPosition.y + sin(ang) * R);
63 
64  outSample.phi(
65  getRandomGenerator().drawGaussian1D(m_oldPose.phi(), 2.0_deg));
66 
67  // Compute the robot pose P.
68  // P = SAMPLE - ROT · SENSOR_ON_ROBOT
70  m_dataPerBeacon[m_drawIndex].sensorOnRobot.x,
71  m_dataPerBeacon[m_drawIndex].sensorOnRobot.y);
72  mrpt::math::TPoint2D S(outSample.x(), outSample.y());
73  on = mrpt::math::TPoint2D(mrpt::math::TPose2D(0, 0, outSample.phi()) + on);
74  S = S - on;
75 
76  outSample.x(S.x);
77  outSample.y(S.y);
78 
79  MRPT_END
80 }
81 
82 /*---------------------------------------------------------------
83  RS_observationLikelihood
84 ---------------------------------------------------------------*/
86  const CPose2D& x)
87 {
89 
90  double lik = 1.0;
91  double m_sigmaRanges2 = square(m_sigmaRanges);
92 
93  // Evaluate the likelihood for all the observations but the "m_drawIndex":
94  for (size_t i = 0; i < m_dataPerBeacon.size(); i++)
95  {
96  // TODO: height now includes the sensor "z"!!!...
97  CPoint3D P(
98  x + CPoint3D(
99  m_dataPerBeacon[i].sensorOnRobot.x,
100  m_dataPerBeacon[i].sensorOnRobot.y, 0));
101 
102  if (i != m_drawIndex)
103  // Evalute:
104  lik *=
105  exp(-0.5 *
106  square(
107  m_dataPerBeacon[i].radiusAtRobotPlane -
108  P.distanceTo(m_dataPerBeacon[i].beaconPosition)) /
109  m_sigmaRanges2);
110  }
111 
112  return lik;
113 
114  MRPT_END
115 }
116 
117 /*---------------------------------------------------------------
118  setParams
119 ---------------------------------------------------------------*/
121  const CLandmarksMap& beaconsMap,
122  const CObservationBeaconRanges& observation, float sigmaRanges,
123  const CPose2D& oldPose, float robot_z, bool autoCheckAngleRanges)
124 
125 {
126  MRPT_START
127 
128  // Various vars:
129  m_z_robot = robot_z;
130  m_oldPose = oldPose;
131  m_sigmaRanges = sigmaRanges; // TODO: observation.stdError;
132 
133  double xMin = 1e30, xMax = -1e30, yMin = 1e30, yMax = -1e30,
134  gridRes = 2 * m_sigmaRanges;
135  float R;
136 
137  // Save each observed beacon data:
138  m_dataPerBeacon.clear();
139 
140  // Minimum radius:
141  std::deque<
143  size_t i;
144 
145  // For each observation:
146  for (i = 0, it = observation.sensedData.begin();
147  it != observation.sensedData.end(); ++it, ++i)
148  {
149  // Is in the map?
150  const CLandmark* lm = beaconsMap.landmarks.getByBeaconID(it->beaconID);
151  if (lm != nullptr)
152  {
154 
155  data.sensorOnRobot = it->sensorLocationOnRobot.asTPoint();
156 
157  data.beaconPosition.x = lm->pose_mean.x;
158  data.beaconPosition.y = lm->pose_mean.y;
159 
160  // First compute squared:
161  data.radiusAtRobotPlane =
162  square(it->sensedDistance) - square(lm->pose_mean.z - robot_z);
163 
164  if (data.radiusAtRobotPlane > 0)
165  {
166  data.radiusAtRobotPlane = sqrt(data.radiusAtRobotPlane);
167  data.minAngle = -M_PIf;
168  data.maxAngle = M_PIf;
169  m_dataPerBeacon.push_back(data);
170 
171  // std::cout << "BEACON: " << m_dataPerBeacon.size() << "\n "
172  // <<data.beaconPosition << " R=" << data.radiusAtRobotPlane <<
173  // "\n";
174 
175  // Keep max/min:
176  xMin = min(
177  xMin, data.beaconPosition.x - (data.radiusAtRobotPlane +
178  4 * m_sigmaRanges + 1.0));
179  xMax = max(
180  xMax, data.beaconPosition.x + (data.radiusAtRobotPlane +
181  4 * m_sigmaRanges + 1.0));
182  yMin = min(
183  yMin, data.beaconPosition.y - (data.radiusAtRobotPlane +
184  4 * m_sigmaRanges + 1.0));
185  yMax = max(
186  yMax, data.beaconPosition.y + (data.radiusAtRobotPlane +
187  4 * m_sigmaRanges + 1.0));
188  }
189  else
190  {
191  printf(
192  "\nWARNING: Beacon range is shorter than distance between "
193  "the robot and the beacon in the Z axis!!!: Ignoring this "
194  "measurement\n");
195  }
196  }
197  } // end for
198 
199  // ------------------------------------------------------------------------
200  // Precompute the min/max angles for potential samples, for each beacon:
201  // ------------------------------------------------------------------------
202  if (autoCheckAngleRanges && m_dataPerBeacon.size() > 1)
203  {
204  // Build the grid:
205  // Each cell in the grid is a vector of bools, each one indicating
206  // whether some samples from the i'th beacon falls there.
208  xMin, xMax, yMin, yMax, gridRes);
209  grid.fill(std::vector<bool>(m_dataPerBeacon.size(), false));
210  std::vector<bool>* cell;
211 
212  // The ngular step size:
213  float Aa = 5.0_deg;
214 
215  // Fill the grid:
216  for (i = 0; i < m_dataPerBeacon.size(); i++)
217  {
218  for (float a = -M_PIf; a <= M_PIf; a += Aa)
219  {
220  // Radius R - 3*SIGMA:
221  R = m_dataPerBeacon[i].radiusAtRobotPlane -
222  3.0f * m_sigmaRanges;
223  R = max(R, 0.0f);
224  cell = grid.cellByPos(
225  m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
226  m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
227  ASSERT_(cell != nullptr);
228  (*cell)[i] = true;
229  // Radius R :
230  R = m_dataPerBeacon[i].radiusAtRobotPlane;
231  cell = grid.cellByPos(
232  m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
233  m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
234  ASSERT_(cell != nullptr);
235  (*cell)[i] = true;
236  // Radius R + 3*SIGMA:
237  R = m_dataPerBeacon[i].radiusAtRobotPlane +
238  3.0f * m_sigmaRanges;
239  cell = grid.cellByPos(
240  m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
241  m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
242  ASSERT_(cell != nullptr);
243  (*cell)[i] = true;
244  } // end for a
245  } // end for i
246 
247  // Check the angles:
248  for (i = 0; i < m_dataPerBeacon.size(); i++)
249  {
250  float maxA = -M_PIf, minA = M_PIf;
251  for (float a = -M_PIf; a < M_PIf; a += Aa)
252  {
253  // Radius R - 3*SIGMA:
254  R = m_dataPerBeacon[i].radiusAtRobotPlane -
255  3.0f * m_sigmaRanges;
256  R = max(R, 0.0f);
257  cell = grid.cellByPos(
258  m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
259  m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
260  ASSERT_(cell != nullptr);
261  if (std::count(cell->begin(), cell->end(), true) > 1)
262  {
263  maxA = max(maxA, a);
264  minA = min(minA, a);
265  }
266  // Radius R :
267  R = m_dataPerBeacon[i].radiusAtRobotPlane;
268  cell = grid.cellByPos(
269  m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
270  m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
271  ASSERT_(cell != nullptr);
272  if (std::count(cell->begin(), cell->end(), true) > 1)
273  {
274  maxA = max(maxA, a);
275  minA = min(minA, a);
276  }
277  // Radius R + 3*SIGMA:
278  R = m_dataPerBeacon[i].radiusAtRobotPlane +
279  3.0f * m_sigmaRanges;
280  cell = grid.cellByPos(
281  m_dataPerBeacon[i].beaconPosition.x + cos(a) * R,
282  m_dataPerBeacon[i].beaconPosition.y + sin(a) * R);
283  ASSERT_(cell != nullptr);
284  if (std::count(cell->begin(), cell->end(), true) > 1)
285  {
286  maxA = max(maxA, a);
287  minA = min(minA, a);
288  }
289  } // end for a
290  m_dataPerBeacon[i].minAngle = max(-M_PIf, minA - Aa);
291  m_dataPerBeacon[i].maxAngle = min(M_PIf, maxA + Aa);
292  } // end for i
293  } // end if >1 beacons
294 
295  // Select best candidate for "m_drawIndex":
296  float minCoberture = 1e30f;
297  m_drawIndex = 0;
298  for (i = 0; i < m_dataPerBeacon.size(); i++)
299  {
300  float c = m_dataPerBeacon[i].radiusAtRobotPlane *
301  (m_dataPerBeacon[i].maxAngle - m_dataPerBeacon[i].minAngle);
302  if (c < minCoberture)
303  {
304  minCoberture = c;
305  m_drawIndex = i;
306  }
307  } // end for i
308 
309  // End!
310  return m_dataPerBeacon.size() != 0;
311  MRPT_END
312 }
A namespace of pseudo-random numbers generators of diferent distributions.
#define MRPT_START
Definition: exceptions.h:241
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:109
TPoint2D_< double > TPoint2D
Lightweight 2D point.
Definition: TPoint2D.h:213
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
STL namespace.
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or nullptr if not found.
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
size_t m_drawIndex
The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
This base provides a set of functions for maths stuff.
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
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or nullptr if it is out of the ...
Definition: CDynamicGrid.h:201
std::deque< TMeasurement > sensedData
The list of observed ranges.
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
return_t drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
A class used to store a 3D point.
Definition: CPoint3D.h:31
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::deque< TDataPerBeacon > m_dataPerBeacon
Data for each beacon observation with a correspondence with the map.
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
double RS_observationLikelihood(const mrpt::poses::CPose2D &x) override
Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the st...
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const float R
#define MRPT_END
Definition: exceptions.h:245
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
static struct FontData data
Definition: gltext.cpp:144



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