Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
12 
15 #include <mrpt/maps/CLandmark.h>
17 #include <mrpt/math/utils.h>
18 
19 using namespace mrpt::utils;
20 using namespace mrpt::math;
21 using namespace mrpt::slam;
22 using namespace mrpt::maps;
23 using namespace mrpt::obs;
24 using namespace mrpt::random;
25 using namespace mrpt::poses;
26 using namespace std;
27 
28 /*---------------------------------------------------------------
29  Constructor
30 ---------------------------------------------------------------*/
31 CRejectionSamplingRangeOnlyLocalization::CRejectionSamplingRangeOnlyLocalization() :
32  m_z_robot ( 0 ),
33  m_sigmaRanges ( 0.10f ),
34  m_oldPose(),
35  m_drawIndex(0),
36  m_dataPerBeacon()
37 {
38 }
39 
40 /*---------------------------------------------------------------
41  RS_drawFromProposal
42 ---------------------------------------------------------------*/
44 {
46 
47  // Use the first beacon data to draw a pose:
48  if (m_dataPerBeacon.size()==0)
49  THROW_EXCEPTION("There is no information from which to draw samples!! Use 'setParams' with valid data!");
50 
52 
55 
56  // This is the point where the SENSOR is:
57  outSample.x( m_dataPerBeacon[m_drawIndex].beaconPosition.x + cos(ang) * R );
58  outSample.y( m_dataPerBeacon[m_drawIndex].beaconPosition.y + sin(ang) * R );
59 
60  outSample.phi( randomGenerator.drawGaussian1D( m_oldPose.phi(), DEG2RAD(2) ) );
61 
62  // Compute the robot pose P.
63  // P = SAMPLE - ROT ยท SENSOR_ON_ROBOT
64  mrpt::math::TPoint2D on(m_dataPerBeacon[m_drawIndex].sensorOnRobot.x,m_dataPerBeacon[m_drawIndex].sensorOnRobot.y);
65  mrpt::math::TPoint2D S(outSample.x(),outSample.y());
66  on = mrpt::math::TPoint2D(mrpt::math::TPose2D(0,0,outSample.phi()) + on);
67  S = S - on;
68 
69  outSample.x( S.x );
70  outSample.y( S.y );
71 
72  MRPT_END
73 }
74 
75 /*---------------------------------------------------------------
76  RS_observationLikelihood
77 ---------------------------------------------------------------*/
79 {
81 
82  double lik = 1.0;
83  double m_sigmaRanges2 = square(m_sigmaRanges);
84 
85  // Evaluate the likelihood for all the observations but the "m_drawIndex":
86  for (size_t i=0;i<m_dataPerBeacon.size();i++)
87  {
88  // TODO: height now includes the sensor "z"!!!...
89  CPoint3D P( x + CPoint3D(m_dataPerBeacon[i].sensorOnRobot.x,m_dataPerBeacon[i].sensorOnRobot.y,0) );
90 
91  if (i!=m_drawIndex)
92  // Evalute:
93  lik*=exp(-0.5* square( m_dataPerBeacon[i].radiusAtRobotPlane - P.distanceTo(m_dataPerBeacon[i].beaconPosition) ) / m_sigmaRanges2 );
94  }
95 
96  return lik;
97 
98  MRPT_END
99 }
100 
101 /*---------------------------------------------------------------
102  setParams
103 ---------------------------------------------------------------*/
105  const CLandmarksMap &beaconsMap,
106  const CObservationBeaconRanges &observation,
107  float sigmaRanges,
108  const CPose2D &oldPose,
109  float robot_z,
110  bool autoCheckAngleRanges )
111 
112 {
113  MRPT_START
114 
115  // Various vars:
116  m_z_robot = robot_z;
117  m_oldPose = oldPose;
118  m_sigmaRanges = sigmaRanges; // TODO: observation.stdError;
119 
120  double xMin=1e30,xMax=-1e30,yMin=1e30,yMax=-1e30,gridRes=2*m_sigmaRanges;
121  float R;
122 
123  // Save each observed beacon data:
124  m_dataPerBeacon.clear();
125 
126  // Minimum radius:
128  size_t i;
129 
130  // For each observation:
131  for (i=0,it=observation.sensedData.begin();it!=observation.sensedData.end();++it,++i)
132  {
133  // Is in the map?
134  const CLandmark *lm = beaconsMap.landmarks.getByBeaconID( it->beaconID );
135  if (lm!=NULL)
136  {
138 
139  data.sensorOnRobot = it->sensorLocationOnRobot;
140 
141  data.beaconPosition.x = lm->pose_mean.x;
142  data.beaconPosition.y = lm->pose_mean.y;
143 
144  // First compute squared:
145  data.radiusAtRobotPlane = square(it->sensedDistance) - square( lm->pose_mean.z - robot_z );
146 
147  if (data.radiusAtRobotPlane>0)
148  {
149  data.radiusAtRobotPlane = sqrt( data.radiusAtRobotPlane );
150  data.minAngle=-M_PIf;
151  data.maxAngle= M_PIf;
152  m_dataPerBeacon.push_back(data);
153 
154  //std::cout << "BEACON: " << m_dataPerBeacon.size() << "\n " <<data.beaconPosition << " R=" << data.radiusAtRobotPlane << "\n";
155 
156  // Keep max/min:
157  xMin=min(xMin,data.beaconPosition.x-(data.radiusAtRobotPlane+4*m_sigmaRanges+1.0));
158  xMax=max(xMax,data.beaconPosition.x+(data.radiusAtRobotPlane+4*m_sigmaRanges+1.0));
159  yMin=min(yMin,data.beaconPosition.y-(data.radiusAtRobotPlane+4*m_sigmaRanges+1.0));
160  yMax=max(yMax,data.beaconPosition.y+(data.radiusAtRobotPlane+4*m_sigmaRanges+1.0));
161  }
162  else
163  {
164  printf("\nWARNING: Beacon range is shorter than distance between the robot and the beacon in the Z axis!!!: Ignoring this measurement\n");
165  }
166  }
167  } // end for
168 
169  // ------------------------------------------------------------------------
170  // Precompute the min/max angles for potential samples, for each beacon:
171  // ------------------------------------------------------------------------
172  if (autoCheckAngleRanges && m_dataPerBeacon.size()>1)
173  {
174  // Build the grid:
175  // Each cell in the grid is a vector of bools, each one indicating whether some samples from the i'th beacon falls there.
176  utils::CDynamicGrid<vector_bool> grid(xMin,xMax,yMin,yMax,gridRes);
177  grid.fill(vector_bool(m_dataPerBeacon.size(),false));
178  vector_bool *cell;
179 
180  // The ngular step size:
181  float Aa = DEG2RAD(5);
182 
183  // Fill the grid:
184  for (i=0;i<m_dataPerBeacon.size();i++)
185  {
186  for (float a=-M_PIf;a<=M_PIf;a+=Aa)
187  {
188  // Radius R - 3*SIGMA:
189  R = m_dataPerBeacon[i].radiusAtRobotPlane - 3.0f*m_sigmaRanges;
190  R = max(R,0.0f);
191  cell=grid.cellByPos(
192  m_dataPerBeacon[i].beaconPosition.x + cos(a)*R,
193  m_dataPerBeacon[i].beaconPosition.y + sin(a)*R ); ASSERT_(cell!=NULL); (*cell)[i]=true;
194  // Radius R :
195  R = m_dataPerBeacon[i].radiusAtRobotPlane;
196  cell=grid.cellByPos(
197  m_dataPerBeacon[i].beaconPosition.x + cos(a)*R,
198  m_dataPerBeacon[i].beaconPosition.y + sin(a)*R ); ASSERT_(cell!=NULL); (*cell)[i]=true;
199  // Radius R + 3*SIGMA:
200  R = m_dataPerBeacon[i].radiusAtRobotPlane + 3.0f*m_sigmaRanges;
201  cell=grid.cellByPos(
202  m_dataPerBeacon[i].beaconPosition.x + cos(a)*R,
203  m_dataPerBeacon[i].beaconPosition.y + sin(a)*R ); ASSERT_(cell!=NULL); (*cell)[i]=true;
204  } // end for a
205  } // end for i
206 
207 
208  // Check the angles:
209  for (i=0;i<m_dataPerBeacon.size();i++)
210  {
211  float maxA=-M_PIf, minA=M_PIf;
212  for (float a=-M_PIf;a<M_PIf;a+=Aa)
213  {
214  // Radius R - 3*SIGMA:
215  R = m_dataPerBeacon[i].radiusAtRobotPlane - 3.0f*m_sigmaRanges;
216  R = max(R,0.0f);
217  cell=grid.cellByPos(
218  m_dataPerBeacon[i].beaconPosition.x + cos(a)*R,
219  m_dataPerBeacon[i].beaconPosition.y + sin(a)*R ); ASSERT_(cell!=NULL);
220  if ( std::count(cell->begin(),cell->end(),true)>1) { maxA=max(maxA,a);minA=min(minA,a); }
221  // Radius R :
222  R = m_dataPerBeacon[i].radiusAtRobotPlane;
223  cell=grid.cellByPos(
224  m_dataPerBeacon[i].beaconPosition.x + cos(a)*R,
225  m_dataPerBeacon[i].beaconPosition.y + sin(a)*R ); ASSERT_(cell!=NULL);
226  if (std::count(cell->begin(),cell->end(),true)>1) { maxA=max(maxA,a);minA=min(minA,a); }
227  // Radius R + 3*SIGMA:
228  R = m_dataPerBeacon[i].radiusAtRobotPlane + 3.0f*m_sigmaRanges;
229  cell=grid.cellByPos(
230  m_dataPerBeacon[i].beaconPosition.x + cos(a)*R,
231  m_dataPerBeacon[i].beaconPosition.y + sin(a)*R ); ASSERT_(cell!=NULL);
232  if (std::count(cell->begin(),cell->end(),true)>1) { maxA=max(maxA,a);minA=min(minA,a); }
233  } // end for a
234  m_dataPerBeacon[i].minAngle=max(-M_PIf, minA-Aa );
235  m_dataPerBeacon[i].maxAngle=min( M_PIf, maxA+Aa );
236  } // end for i
237  } // end if >1 beacons
238 
239  // Select best candidate for "m_drawIndex":
240  float minCoberture=1e30f;
241  m_drawIndex = 0;
242  for (i=0;i<m_dataPerBeacon.size();i++)
243  {
244  float c = m_dataPerBeacon[i].radiusAtRobotPlane*(m_dataPerBeacon[i].maxAngle - m_dataPerBeacon[i].minAngle);
245  if (c<minCoberture)
246  {
247  minCoberture = c;
248  m_drawIndex = i;
249  }
250  } // end for i
251 
252  // End!
253  return m_dataPerBeacon.size()!=0;
254  MRPT_END
255 }
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
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 y
X,Y coordinates.
#define min(a, b)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
const GLfloat * c
Definition: glew.h:10088
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
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.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
double z
X,Y,Z coordinates.
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:101
double drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
std::vector< bool > vector_bool
A type for passing a vector of bools.
Definition: types_simple.h:29
size_t m_drawIndex
The index in "m_dataPerBeacon" used to draw samples (the rest will be used to evaluate the likelihood...
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map...
Definition: CDynamicGrid.h:183
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 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define M_PIf
A class for storing a map of 3D probabilistic landmarks.
#define MRPT_END
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.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
Definition: glew.h:1284
#define DEG2RAD
A class used to store a 3D point.
Definition: CPoint3D.h:32
std::deque< TDataPerBeacon > m_dataPerBeacon
Data for each beacon observation with a correspondence with the map.
#define MRPT_START
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or NULL if not found.
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
Lightweight 2D pose.
#define ASSERT_(f)
GLuint GLuint GLsizei count
Definition: glew.h:1167
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Lightweight 2D point.
struct VISION_IMPEXP mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018