MRPT  1.9.9
CRejectionSamplingRangeOnlyLocalization.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "slam-precomp.h" // Precompiled headers
11 
14 #include <mrpt/maps/CLandmark.h>
16 #include <mrpt/math/utils.h>
17 
18 using namespace mrpt::math;
19 using namespace mrpt::slam;
20 using namespace mrpt::maps;
21 using namespace mrpt::obs;
22 using namespace mrpt::random;
23 using namespace mrpt::poses;
24 using namespace std;
25 
26 /*---------------------------------------------------------------
27  Constructor
28 ---------------------------------------------------------------*/
29 CRejectionSamplingRangeOnlyLocalization::
30  CRejectionSamplingRangeOnlyLocalization()
31  : m_z_robot(0),
32  m_sigmaRanges(0.10f),
33  m_oldPose(),
34  m_drawIndex(0),
35  m_dataPerBeacon()
36 {
37 }
38 
39 /*---------------------------------------------------------------
40  RS_drawFromProposal
41 ---------------------------------------------------------------*/
43  CPose2D& outSample)
44 {
46 
47  // Use the first beacon data to draw a pose:
48  if (m_dataPerBeacon.size() == 0)
50  "There is no information from which to draw samples!! Use "
51  "'setParams' with valid data!");
52 
54 
55  float ang = getRandomGenerator().drawUniform(
56  m_dataPerBeacon[m_drawIndex].minAngle,
57  m_dataPerBeacon[m_drawIndex].maxAngle);
59  m_dataPerBeacon[m_drawIndex].radiusAtRobotPlane, m_sigmaRanges);
60 
61  // This is the point where the SENSOR is:
62  outSample.x(m_dataPerBeacon[m_drawIndex].beaconPosition.x + cos(ang) * R);
63  outSample.y(m_dataPerBeacon[m_drawIndex].beaconPosition.y + sin(ang) * R);
64 
65  outSample.phi(
66  getRandomGenerator().drawGaussian1D(m_oldPose.phi(), DEG2RAD(2)));
67 
68  // Compute the robot pose P.
69  // P = SAMPLE - ROT ยท SENSOR_ON_ROBOT
71  m_dataPerBeacon[m_drawIndex].sensorOnRobot.x,
72  m_dataPerBeacon[m_drawIndex].sensorOnRobot.y);
73  mrpt::math::TPoint2D S(outSample.x(), outSample.y());
74  on = mrpt::math::TPoint2D(mrpt::math::TPose2D(0, 0, outSample.phi()) + on);
75  S = S - on;
76 
77  outSample.x(S.x);
78  outSample.y(S.y);
79 
80  MRPT_END
81 }
82 
83 /*---------------------------------------------------------------
84  RS_observationLikelihood
85 ---------------------------------------------------------------*/
87  const CPose2D& x)
88 {
90 
91  double lik = 1.0;
92  double m_sigmaRanges2 = square(m_sigmaRanges);
93 
94  // Evaluate the likelihood for all the observations but the "m_drawIndex":
95  for (size_t i = 0; i < m_dataPerBeacon.size(); i++)
96  {
97  // TODO: height now includes the sensor "z"!!!...
98  CPoint3D P(
99  x + CPoint3D(
100  m_dataPerBeacon[i].sensorOnRobot.x,
101  m_dataPerBeacon[i].sensorOnRobot.y, 0));
102 
103  if (i != m_drawIndex)
104  // Evalute:
105  lik *= exp(
106  -0.5 * 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 = DEG2RAD(5);
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.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
GLuint GLuint GLsizei count
Definition: glext.h:3528
#define MRPT_START
Definition: exceptions.h:262
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
#define min(a, b)
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:117
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:211
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
double DEG2RAD(const double x)
Degrees to radians.
void RS_drawFromProposal(mrpt::poses::CPose2D &outSample)
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 drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
size_t m_drawIndex
The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood...
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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:75
const GLubyte * c
Definition: glext.h:6313
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:211
double RS_observationLikelihood(const mrpt::poses::CPose2D &x)
Returns the NORMALIZED observation likelihood (linear, not exponential!!!) at a given point of the st...
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
X,Y,Z coordinates.
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.
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:33
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
const float R
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:80
#define MRPT_END
Definition: exceptions.h:266
Lightweight 2D pose.
GLenum GLint x
Definition: glext.h:3538
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:45
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Lightweight 2D point.
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3546
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
const Scalar * const_iterator
Definition: eigen_plugins.h:27
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020