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-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 "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/core/round.h> // round()
14 #include <mrpt/math/CVectorFixed.h>
18 #include <mrpt/random.h>
19 #include <Eigen/Dense>
20 
21 using namespace mrpt;
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 
29 
30 // See docs in header
32  mrpt::obs::CObservation2DRangeScan& inout_Scan, const CPose2D& robotPose,
33  float threshold, size_t N, float noiseStd, unsigned int decimation,
34  float angleNoiseStd) const
35 {
37 
38  ASSERT_(decimation >= 1);
39  ASSERT_(N >= 2);
40 
41  // Sensor pose in global coordinates
42  CPose3D sensorPose3D = CPose3D(robotPose) + inout_Scan.sensorPose;
43  // Aproximation: grid is 2D !!!
44  CPose2D sensorPose(sensorPose3D);
45 
46  // Scan size:
47  inout_Scan.resizeScan(N);
48 
49  double A = sensorPose.phi() +
50  (inout_Scan.rightToLeft ? -0.5 : +0.5) * inout_Scan.aperture;
51  const double AA =
52  (inout_Scan.rightToLeft ? 1.0 : -1.0) * (inout_Scan.aperture / (N - 1));
53 
54  const float free_thres = 1.0f - threshold;
55 
56  for (size_t i = 0; i < N; i += decimation, A += AA * decimation)
57  {
58  bool valid;
59  float out_range;
60  simulateScanRay(
61  sensorPose.x(), sensorPose.y(), A, out_range, valid,
62  inout_Scan.maxRange, free_thres, noiseStd, angleNoiseStd);
63  inout_Scan.setScanRange(i, out_range);
64  inout_Scan.setScanRangeValidity(i, valid);
65  }
66 
67  MRPT_END
68 }
69 
71  CObservationRange& inout_observation, const CPose2D& robotPose,
72  float threshold, float rangeNoiseStd, float angleNoiseStd) const
73 {
74  const float free_thres = 1.0f - threshold;
75 
76  for (auto itR = inout_observation.begin(); itR != inout_observation.end();
77  ++itR)
78  {
79  const CPose2D sensorAbsolutePose =
80  CPose2D(CPose3D(robotPose) + CPose3D(itR->sensorPose));
81 
82  // For each sonar cone, simulate several rays and keep the shortest
83  // distance:
84  ASSERT_(inout_observation.sensorConeApperture > 0);
85  size_t nRays =
86  round(1 + inout_observation.sensorConeApperture / 1.0_deg);
87 
88  double direction = sensorAbsolutePose.phi() -
89  0.5 * inout_observation.sensorConeApperture;
90  const double Adir = inout_observation.sensorConeApperture / nRays;
91 
92  float min_detected_obs = 0;
93  for (size_t i = 0; i < nRays; i++, direction += Adir)
94  {
95  bool valid;
96  float sim_rang;
97  simulateScanRay(
98  sensorAbsolutePose.x(), sensorAbsolutePose.y(), direction,
99  sim_rang, valid, inout_observation.maxSensorDistance,
100  free_thres, rangeNoiseStd, angleNoiseStd);
101 
102  if (valid && (sim_rang < min_detected_obs || !i))
103  min_detected_obs = sim_rang;
104  }
105  // Save:
106  itR->sensedDistance = min_detected_obs;
107  }
108 }
109 
111  const double start_x, const double start_y, const double angle_direction,
112  float& out_range, bool& out_valid, const double max_range_meters,
113  const float threshold_free, const double noiseStd,
114  const double angleNoiseStd) const
115 {
116  const double A_ =
117  angle_direction +
118  (angleNoiseStd > .0
119  ? getRandomGenerator().drawGaussian1D_normalized() * angleNoiseStd
120  : .0);
121 
122 // Unit vector in the directorion of the ray:
123 #ifdef HAVE_SINCOS
124  double Arx, Ary;
125  ::sincos(A_, &Ary, &Arx);
126 #else
127  const double Arx = cos(A_);
128  const double Ary = sin(A_);
129 #endif
130 
131  // Ray tracing, until collision, out of the map or out of range:
132  const unsigned int max_ray_len = mrpt::round(max_range_meters / resolution);
133  unsigned int ray_len = 0;
134 
135 // Use integers for all ray tracing for efficiency
136 #define INTPRECNUMBIT 10
137 #define int_x2idx(_X) (_X >> INTPRECNUMBIT)
138 #define int_y2idx(_Y) (_Y >> INTPRECNUMBIT)
139 
140  auto rxi = static_cast<int64_t>(
141  ((start_x - x_min) / resolution) * (1L << INTPRECNUMBIT));
142  auto ryi = static_cast<int64_t>(
143  ((start_y - y_min) / resolution) * (1L << INTPRECNUMBIT));
144 
145  const auto Arxi = static_cast<int64_t>(
146  RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Arx * (1L << INTPRECNUMBIT));
147  const auto Aryi = static_cast<int64_t>(
148  RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Ary * (1L << INTPRECNUMBIT));
149 
150  cellType hitCellOcc_int = 0; // p2l(0.5f)
151  const cellType threshold_free_int = p2l(threshold_free);
152  int x, y = int_y2idx(ryi);
153 
154  while ((x = int_x2idx(rxi)) >= 0 && (y = int_y2idx(ryi)) >= 0 &&
155  x < static_cast<int>(size_x) && y < static_cast<int>(size_y) &&
156  (hitCellOcc_int = map[x + y * size_x]) > threshold_free_int &&
157  ray_len < max_ray_len)
158  {
159  rxi += Arxi;
160  ryi += Aryi;
161  ray_len++;
162  }
163 
164  // Store:
165  // Check out of the grid?
166  // Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
167  if (std::abs(hitCellOcc_int) <= 1 || static_cast<unsigned>(x) >= size_x ||
168  static_cast<unsigned>(y) >= size_y)
169  {
170  out_valid = false;
171  out_range = max_range_meters;
172  }
173  else
174  { // No: The normal case:
175  out_range = RAYTRACE_STEP_SIZE_IN_CELL_UNITS * ray_len * resolution;
176  out_valid = (ray_len < max_ray_len); // out_range<max_range_meters;
177  // Add additive Gaussian noise:
178  if (noiseStd > 0 && out_valid)
179  out_range +=
181  }
182 }
183 
186 
187  = default;
188 
191 
193 {
196 };
197 
199  const mrpt::math::CVectorFixedDouble<3>& x_pose,
200  const TFunctorLaserSimulData& fixed_param,
201  mrpt::math::CVectorDouble& y_scanRanges)
202 {
203  ASSERT_(fixed_param.params && fixed_param.grid);
204  ASSERT_(fixed_param.params->decimation >= 1);
205  ASSERT_(fixed_param.params->nRays >= 2);
206 
207  const size_t N = fixed_param.params->nRays;
208 
209  // Sensor pose in global coordinates
210  const CPose3D sensorPose3D =
211  CPose3D(x_pose[0], x_pose[1], .0, x_pose[2], .0, .0) +
212  fixed_param.params->sensorPose;
213  // Aproximation: grid is 2D !!!
214  const CPose2D sensorPose(sensorPose3D);
215 
216  // Scan size:
217  y_scanRanges.resize(N);
218 
219  double A =
220  sensorPose.phi() + (fixed_param.params->rightToLeft ? -0.5 : +0.5) *
221  fixed_param.params->aperture;
222  const double AA = (fixed_param.params->rightToLeft ? 1.0 : -1.0) *
223  (fixed_param.params->aperture / (N - 1));
224 
225  const float free_thres = 1.0f - fixed_param.params->threshold;
226 
227  for (size_t i = 0; i < N; i += fixed_param.params->decimation,
228  A += AA * fixed_param.params->decimation)
229  {
230  bool valid;
231  float range;
232 
233  fixed_param.grid->simulateScanRay(
234  sensorPose.x(), sensorPose.y(), A, range, valid,
235  fixed_param.params->maxRange, free_thres, .0 /*noiseStd*/,
236  .0 /*angleNoiseStd*/);
237  y_scanRanges[i] = valid ? range : fixed_param.params->maxRange;
238  }
239 }
240 
244 {
245  const mrpt::math::CVectorFixedDouble<3> robPoseMean =
246  in_params.robotPose.mean.asVectorVal();
247 
248  TFunctorLaserSimulData simulData;
249  simulData.grid = this;
250  simulData.params = &in_params;
251 
252  switch (in_params.method)
253  {
254  case sumUnscented:
256  robPoseMean, // x_mean
257  in_params.robotPose.cov, // x_cov
258  &func_laserSimul_callback, // void (*functor)(const
259  // VECTORLIKE1 &x,const USERPARAM
260  // &fixed_param, VECTORLIKE3 &y)
261  simulData, // const USERPARAM &fixed_param,
262  out_results.scanWithUncert.rangesMean,
263  out_results.scanWithUncert.rangesCovar,
264  nullptr, // elem_do_wrap2pi,
265  in_params.UT_alpha, in_params.UT_kappa,
266  in_params.UT_beta // alpha, K, beta
267  );
268  break;
269  case sumMonteCarlo:
270  //
272  robPoseMean, // x_mean
273  in_params.robotPose.cov, // x_cov
274  &func_laserSimul_callback, // void (*functor)(const
275  // VECTORLIKE1 &x,const USERPARAM
276  // &fixed_param, VECTORLIKE3 &y)
277  simulData, // const USERPARAM &fixed_param,
278  out_results.scanWithUncert.rangesMean,
279  out_results.scanWithUncert.rangesCovar, in_params.MC_samples);
280  break;
281  default:
282  throw std::runtime_error(
283  "[laserScanSimulatorWithUncertainty] Unknown `method` value");
284  break;
285  };
286 
287  // Outputs:
288  out_results.scanWithUncert.rangeScan.aperture = in_params.aperture;
289  out_results.scanWithUncert.rangeScan.maxRange = in_params.maxRange;
290  out_results.scanWithUncert.rangeScan.rightToLeft = in_params.rightToLeft;
291  out_results.scanWithUncert.rangeScan.sensorPose = in_params.sensorPose;
292 
293  out_results.scanWithUncert.rangeScan.resizeScan(in_params.nRays);
294  for (unsigned i = 0; i < in_params.nRays; i++)
295  {
297  i, (float)out_results.scanWithUncert.rangesMean[i]);
298  out_results.scanWithUncert.rangeScan.setScanRangeValidity(i, true);
299  }
300 
301  // Add minimum uncertainty: grid cell resolution:
302  out_results.scanWithUncert.rangesCovar.asEigen().diagonal().array() +=
303  0.5 * resolution * resolution;
304 }
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.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
static void func_laserSimul_callback(const mrpt::math::CVectorFixedDouble< 3 > &x_pose, const TFunctorLaserSimulData &fixed_param, mrpt::math::CVectorDouble &y_scanRanges)
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
#define MRPT_START
Definition: exceptions.h:241
CPose2D mean
The mean value.
float threshold
(Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied ...
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
mrpt::obs::CObservation2DRangeScanWithUncertainty scanWithUncert
The scan + its uncertainty.
mrpt::poses::CPose3D sensorPose
(Default: at origin) The 6D pose of the sensor on the robot at the moment of starting the scan...
#define INTPRECNUMBIT
void setScanRange(const size_t i, const float val)
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.
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, std::vector< VECTORLIKE3 > *out_samples_y=nullptr)
Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary ...
#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:120
mrpt::math::CVectorDouble rangesMean
The same ranges than in rangeScan.getScanRange(), for convenience as a math vector container...
size_t MC_samples
[sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
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:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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.
mrpt::math::CMatrixDouble rangesCovar
The covariance matrix for all the ranges in rangeScan.getScanRange()
#define MRPT_END
Definition: exceptions.h:245
TLaserSimulUncertaintyMethod method
(Default: sumMonteCarlo) Select the method to do the uncertainty propagation
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...
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g.
void resize(std::size_t N, bool zeroNewElements=false)
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
vector_t asVectorVal() const
Return the pose or point as a 1xN vector with all the components (see derived classes for each implem...
Definition: CPoseOrPoint.h:266
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:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020