MRPT  1.9.9
COccupancyGridMap2D_simulate.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-2019, 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 "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/core/round.h> // round()
17 
18 #include <mrpt/random.h>
19 
20 using namespace mrpt;
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 
28 
29 // See docs in header
31  mrpt::obs::CObservation2DRangeScan& inout_Scan, const CPose2D& robotPose,
32  float threshold, size_t N, float noiseStd, unsigned int decimation,
33  float angleNoiseStd) const
34 {
36 
37  ASSERT_(decimation >= 1);
38  ASSERT_(N >= 2);
39 
40  // Sensor pose in global coordinates
41  CPose3D sensorPose3D = CPose3D(robotPose) + inout_Scan.sensorPose;
42  // Aproximation: grid is 2D !!!
43  CPose2D sensorPose(sensorPose3D);
44 
45  // Scan size:
46  inout_Scan.resizeScan(N);
47 
48  double A = sensorPose.phi() +
49  (inout_Scan.rightToLeft ? -0.5 : +0.5) * inout_Scan.aperture;
50  const double AA =
51  (inout_Scan.rightToLeft ? 1.0 : -1.0) * (inout_Scan.aperture / (N - 1));
52 
53  const float free_thres = 1.0f - threshold;
54 
55  for (size_t i = 0; i < N; i += decimation, A += AA * decimation)
56  {
57  bool valid;
58  float out_range;
59  simulateScanRay(
60  sensorPose.x(), sensorPose.y(), A, out_range, valid,
61  inout_Scan.maxRange, free_thres, noiseStd, angleNoiseStd);
62  inout_Scan.setScanRange(i, out_range);
63  inout_Scan.setScanRangeValidity(i, valid);
64  }
65 
66  MRPT_END
67 }
68 
70  CObservationRange& inout_observation, const CPose2D& robotPose,
71  float threshold, float rangeNoiseStd, float angleNoiseStd) const
72 {
73  const float free_thres = 1.0f - threshold;
74 
75  for (auto itR = inout_observation.begin(); itR != inout_observation.end();
76  ++itR)
77  {
78  const CPose2D sensorAbsolutePose =
79  CPose2D(CPose3D(robotPose) + CPose3D(itR->sensorPose));
80 
81  // For each sonar cone, simulate several rays and keep the shortest
82  // distance:
83  ASSERT_(inout_observation.sensorConeApperture > 0);
84  size_t nRays =
85  round(1 + inout_observation.sensorConeApperture / DEG2RAD(1.0));
86 
87  double direction = sensorAbsolutePose.phi() -
88  0.5 * inout_observation.sensorConeApperture;
89  const double Adir = inout_observation.sensorConeApperture / nRays;
90 
91  float min_detected_obs = 0;
92  for (size_t i = 0; i < nRays; i++, direction += Adir)
93  {
94  bool valid;
95  float sim_rang;
96  simulateScanRay(
97  sensorAbsolutePose.x(), sensorAbsolutePose.y(), direction,
98  sim_rang, valid, inout_observation.maxSensorDistance,
99  free_thres, rangeNoiseStd, angleNoiseStd);
100 
101  if (valid && (sim_rang < min_detected_obs || !i))
102  min_detected_obs = sim_rang;
103  }
104  // Save:
105  itR->sensedDistance = min_detected_obs;
106  }
107 }
108 
110  const double start_x, const double start_y, const double angle_direction,
111  float& out_range, bool& out_valid, const double max_range_meters,
112  const float threshold_free, const double noiseStd,
113  const double angleNoiseStd) const
114 {
115  const double A_ =
116  angle_direction +
117  (angleNoiseStd > .0
118  ? getRandomGenerator().drawGaussian1D_normalized() * angleNoiseStd
119  : .0);
120 
121 // Unit vector in the directorion of the ray:
122 #ifdef HAVE_SINCOS
123  double Arx, Ary;
124  ::sincos(A_, &Ary, &Arx);
125 #else
126  const double Arx = cos(A_);
127  const double Ary = sin(A_);
128 #endif
129 
130  // Ray tracing, until collision, out of the map or out of range:
131  const unsigned int max_ray_len = mrpt::round(max_range_meters / resolution);
132  unsigned int ray_len = 0;
133 
134 // Use integers for all ray tracing for efficiency
135 #define INTPRECNUMBIT 10
136 #define int_x2idx(_X) (_X >> INTPRECNUMBIT)
137 #define int_y2idx(_Y) (_Y >> INTPRECNUMBIT)
138 
139  auto rxi = static_cast<int64_t>(
140  ((start_x - x_min) / resolution) * (1L << INTPRECNUMBIT));
141  auto ryi = static_cast<int64_t>(
142  ((start_y - y_min) / resolution) * (1L << INTPRECNUMBIT));
143 
144  const auto Arxi = static_cast<int64_t>(
145  RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Arx * (1L << INTPRECNUMBIT));
146  const auto Aryi = static_cast<int64_t>(
147  RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Ary * (1L << INTPRECNUMBIT));
148 
149  cellType hitCellOcc_int = 0; // p2l(0.5f)
150  const cellType threshold_free_int = p2l(threshold_free);
151  int x, y = int_y2idx(ryi);
152 
153  while ((x = int_x2idx(rxi)) >= 0 && (y = int_y2idx(ryi)) >= 0 &&
154  x < static_cast<int>(size_x) && y < static_cast<int>(size_y) &&
155  (hitCellOcc_int = map[x + y * size_x]) > threshold_free_int &&
156  ray_len < max_ray_len)
157  {
158  rxi += Arxi;
159  ryi += Aryi;
160  ray_len++;
161  }
162 
163  // Store:
164  // Check out of the grid?
165  // Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
166  if (abs(hitCellOcc_int) <= 1 || static_cast<unsigned>(x) >= size_x ||
167  static_cast<unsigned>(y) >= size_y)
168  {
169  out_valid = false;
170  out_range = max_range_meters;
171  }
172  else
173  { // No: The normal case:
174  out_range = RAYTRACE_STEP_SIZE_IN_CELL_UNITS * ray_len * resolution;
175  out_valid = (ray_len < max_ray_len); // out_range<max_range_meters;
176  // Add additive Gaussian noise:
177  if (noiseStd > 0 && out_valid)
178  out_range +=
180  }
181 }
182 
185 
186  = default;
187 
190 
192 {
195 };
196 
198  const Eigen::Vector3d& x_pose, const TFunctorLaserSimulData& fixed_param,
199  Eigen::VectorXd& y_scanRanges)
200 {
201  ASSERT_(fixed_param.params && fixed_param.grid);
202  ASSERT_(fixed_param.params->decimation >= 1);
203  ASSERT_(fixed_param.params->nRays >= 2);
204 
205  const size_t N = fixed_param.params->nRays;
206 
207  // Sensor pose in global coordinates
208  const CPose3D sensorPose3D =
209  CPose3D(x_pose[0], x_pose[1], .0, x_pose[2], .0, .0) +
210  fixed_param.params->sensorPose;
211  // Aproximation: grid is 2D !!!
212  const CPose2D sensorPose(sensorPose3D);
213 
214  // Scan size:
215  y_scanRanges.resize(N);
216 
217  double A =
218  sensorPose.phi() + (fixed_param.params->rightToLeft ? -0.5 : +0.5) *
219  fixed_param.params->aperture;
220  const double AA = (fixed_param.params->rightToLeft ? 1.0 : -1.0) *
221  (fixed_param.params->aperture / (N - 1));
222 
223  const float free_thres = 1.0f - fixed_param.params->threshold;
224 
225  for (size_t i = 0; i < N; i += fixed_param.params->decimation,
226  A += AA * fixed_param.params->decimation)
227  {
228  bool valid;
229  float range;
230 
231  fixed_param.grid->simulateScanRay(
232  sensorPose.x(), sensorPose.y(), A, range, valid,
233  fixed_param.params->maxRange, free_thres, .0 /*noiseStd*/,
234  .0 /*angleNoiseStd*/);
235  y_scanRanges[i] = valid ? range : fixed_param.params->maxRange;
236  }
237 }
238 
242 {
243  const Eigen::Vector3d robPoseMean =
244  in_params.robotPose.mean.getAsVectorVal();
245 
246  TFunctorLaserSimulData simulData;
247  simulData.grid = this;
248  simulData.params = &in_params;
249 
250  switch (in_params.method)
251  {
252  case sumUnscented:
254  robPoseMean, // x_mean
255  in_params.robotPose.cov, // x_cov
256  &func_laserSimul_callback, // void (*functor)(const
257  // VECTORLIKE1 &x,const USERPARAM
258  // &fixed_param, VECTORLIKE3 &y)
259  simulData, // const USERPARAM &fixed_param,
260  out_results.scanWithUncert.rangesMean,
261  out_results.scanWithUncert.rangesCovar,
262  nullptr, // elem_do_wrap2pi,
263  in_params.UT_alpha, in_params.UT_kappa,
264  in_params.UT_beta // alpha, K, beta
265  );
266  break;
267  case sumMonteCarlo:
268  //
270  robPoseMean, // x_mean
271  in_params.robotPose.cov, // x_cov
272  &func_laserSimul_callback, // void (*functor)(const
273  // VECTORLIKE1 &x,const USERPARAM
274  // &fixed_param, VECTORLIKE3 &y)
275  simulData, // const USERPARAM &fixed_param,
276  out_results.scanWithUncert.rangesMean,
277  out_results.scanWithUncert.rangesCovar, in_params.MC_samples);
278  break;
279  default:
280  throw std::runtime_error(
281  "[laserScanSimulatorWithUncertainty] Unknown `method` value");
282  break;
283  };
284 
285  // Outputs:
286  out_results.scanWithUncert.rangeScan.aperture = in_params.aperture;
287  out_results.scanWithUncert.rangeScan.maxRange = in_params.maxRange;
288  out_results.scanWithUncert.rangeScan.rightToLeft = in_params.rightToLeft;
289  out_results.scanWithUncert.rangeScan.sensorPose = in_params.sensorPose;
290 
291  out_results.scanWithUncert.rangeScan.resizeScan(in_params.nRays);
292  for (unsigned i = 0; i < in_params.nRays; i++)
293  {
295  i, (float)out_results.scanWithUncert.rangesMean[i]);
296  out_results.scanWithUncert.rangeScan.setScanRangeValidity(i, true);
297  }
298 
299  // Add minimum uncertainty: grid cell resolution:
300  out_results.scanWithUncert.rangesCovar.diagonal().array() +=
301  0.5 * resolution * resolution;
302 }
const COccupancyGridMap2D::TLaserSimulUncertaintyParams * params
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates.
A namespace of pseudo-random numbers generators of diferent distributions.
void laserScanSimulator(mrpt::obs::CObservation2DRangeScan &inout_Scan, const mrpt::poses::CPose2D &robotPose, float threshold=0.6f, size_t N=361, float noiseStd=0, unsigned int decimation=1, float angleNoiseStd=mrpt::DEG2RAD(.0)) const
Simulates a laser range scan into the current grid map.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
GLsizei range
Definition: glext.h:5993
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
#define MRPT_START
Definition: exceptions.h:282
CPose2D mean
The mean value.
float threshold
(Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied ...
static void func_laserSimul_callback(const Eigen::Vector3d &x_pose, const TFunctorLaserSimulData &fixed_param, Eigen::VectorXd &y_scanRanges)
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert
The scan + its uncertainty.
mrpt::math::CVectorDouble getAsVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:263
mrpt::poses::CPose3D sensorPose
(Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan...
Eigen::VectorXd rangesMean
The same ranges than in rangeScan.scan[], for convenience as an Eigen container, and with double prec...
#define INTPRECNUMBIT
void setScanRange(const size_t i, const float val)
double DEG2RAD(const double x)
Degrees to radians.
Output params for laserScanSimulatorWithUncertainty()
#define int_y2idx(_Y)
STL namespace.
CObservation2DRangeScan rangeScan
The observation with the mean ranges in the scan field.
bool rightToLeft
(Default: true) The scanning direction: true=counterclockwise; false=clockwise
const COccupancyGridMap2D * grid
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
#define int_x2idx(_X)
float maxRange
The maximum range allowed by the device, in meters (e.g.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:161
size_t MC_samples
[sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
__int64 int64_t
Definition: rptypes.h:52
float aperture
(Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180...
This namespace contains representation of robot actions and observations.
unsigned int decimation
(Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,...
void sonarSimulator(mrpt::obs::CObservationRange &inout_observation, const mrpt::poses::CPose2D &robotPose, float threshold=0.5f, float rangeNoiseStd=0.f, float angleNoiseStd=mrpt::DEG2RAD(0.f)) const
Simulates the observations of a sonar rig into the current grid map.
void transform_gaussian_unscented(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const bool *elem_do_wrap2pi=nullptr, const double alpha=1e-3, const double K=0, const double beta=2.0)
Scaled unscented transformation (SUT) for estimating the Gaussian distribution of a variable Y=f(X) f...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void laserScanSimulatorWithUncertainty(const TLaserSimulUncertaintyParams &in_params, TLaserSimulUncertaintyResult &out_results) const
Like laserScanSimulatorWithUncertainty() (see it for a discussion of most parameters) but taking into...
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
void transform_gaussian_montecarlo(const VECTORLIKE1 &x_mean, const MATLIKE1 &x_cov, void(*functor)(const VECTORLIKE1 &x, const USERPARAM &fixed_param, VECTORLIKE3 &y), const USERPARAM &fixed_param, VECTORLIKE2 &y_mean, MATLIKE2 &y_cov, const size_t num_samples=1000, mrpt::aligned_std_vector< VECTORLIKE3 > *out_samples_y=nullptr)
Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
Input params for laserScanSimulatorWithUncertainty()
void simulateScanRay(const double x, const double y, const double angle_direction, float &out_range, bool &out_valid, const double max_range_meters, const float threshold_free=0.4f, const double noiseStd=.0, const double angleNoiseStd=.0) const
Simulate just one "ray" in the grid map.
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define MRPT_END
Definition: exceptions.h:286
TLaserSimulUncertaintyMethod method
(Default: sumMonteCarlo) Select the method to do the uncertainty propagation
Eigen::MatrixXd rangesCovar
The covariance matrix for all the ranges in rangeScan.scan[].
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
OccGridCellTraits::cellType cellType
The type of the map cells:
static double RAYTRACE_STEP_SIZE_IN_CELL_UNITS
(Default:1.0) Can be set to <1 if a more fine raytracing is needed in sonarSimulator() and laserScanS...
GLenum GLint GLint y
Definition: glext.h:3542
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g.
GLenum GLint x
Definition: glext.h:3542
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
void setScanRangeValidity(const size_t i, const bool val)
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019