Main MRPT website > C++ reference for MRPT 1.5.6
COccupancyGridMap2D_simulate.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 "maps-precomp.h" // Precomp header
11 
15 #include <mrpt/utils/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::utils;
24 using namespace mrpt::random;
25 using namespace mrpt::poses;
26 using namespace std;
27 
29 
30 // See docs in header
33  const CPose2D &robotPose,
34  float threshold,
35  size_t N,
36  float noiseStd,
37  unsigned int decimation,
38  float angleNoiseStd ) const
39 {
41 
42  ASSERT_(decimation>=1)
43  ASSERT_(N>=2)
44 
45  // Sensor pose in global coordinates
46  CPose3D sensorPose3D = CPose3D(robotPose) + inout_Scan.sensorPose;
47  // Aproximation: grid is 2D !!!
48  CPose2D sensorPose(sensorPose3D);
49 
50  // Scan size:
51  inout_Scan.resizeScan(N);
52 
53  double A = sensorPose.phi() + (inout_Scan.rightToLeft ? -0.5:+0.5) *inout_Scan.aperture;
54  const double AA = (inout_Scan.rightToLeft ? 1.0:-1.0) * (inout_Scan.aperture / (N-1));
55 
56  const float free_thres = 1.0f - threshold;
57 
58  for (size_t i=0;i<N;i+=decimation,A+=AA*decimation)
59  {
60  bool valid;
61  float out_range;
62  simulateScanRay(
63  sensorPose.x(),sensorPose.y(),A,
64  out_range,valid,
65  inout_Scan.maxRange, free_thres,
66  noiseStd, angleNoiseStd );
67  inout_Scan.setScanRange( i, out_range);
68  inout_Scan.setScanRangeValidity(i, valid);
69  }
70 
71  MRPT_END
72 }
73 
75  CObservationRange &inout_observation,
76  const CPose2D &robotPose,
77  float threshold,
78  float rangeNoiseStd,
79  float angleNoiseStd) const
80 {
81  const float free_thres = 1.0f - threshold;
82 
83  for (CObservationRange::iterator itR=inout_observation.begin();itR!=inout_observation.end();++itR)
84  {
85  const CPose2D sensorAbsolutePose = CPose2D( CPose3D(robotPose) + CPose3D(itR->sensorPose) );
86 
87  // For each sonar cone, simulate several rays and keep the shortest distance:
88  ASSERT_(inout_observation.sensorConeApperture>0)
89  size_t nRays = round(1+ inout_observation.sensorConeApperture / DEG2RAD(1.0) );
90 
91  double direction = sensorAbsolutePose.phi() - 0.5*inout_observation.sensorConeApperture;
92  const double Adir = inout_observation.sensorConeApperture / nRays;
93 
94  float min_detected_obs=0;
95  for (size_t i=0;i<nRays;i++, direction+=Adir )
96  {
97  bool valid;
98  float sim_rang;
99  simulateScanRay(
100  sensorAbsolutePose.x(), sensorAbsolutePose.y(), direction,
101  sim_rang, valid,
102  inout_observation.maxSensorDistance, free_thres,
103  rangeNoiseStd, angleNoiseStd );
104 
105  if (valid && (sim_rang<min_detected_obs || !i))
106  min_detected_obs = sim_rang;
107  }
108  // Save:
109  itR->sensedDistance = min_detected_obs;
110  }
111 }
112 
114  const double start_x,const double start_y,const double angle_direction,
115  float &out_range,bool &out_valid,
116  const double max_range_meters,
117  const float threshold_free,
118  const double noiseStd, const double angleNoiseStd ) const
119 {
120  const double A_ = angle_direction + (angleNoiseStd>.0 ? randomGenerator.drawGaussian1D_normalized()*angleNoiseStd : .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::utils::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  int64_t rxi = static_cast<int64_t>( ((start_x-x_min)/resolution) * (1L <<INTPRECNUMBIT));
141  int64_t ryi = static_cast<int64_t>( ((start_y-y_min)/resolution) * (1L <<INTPRECNUMBIT));
142 
143  const int64_t Arxi = static_cast<int64_t>( RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Arx * (1L <<INTPRECNUMBIT) );
144  const int64_t Aryi = static_cast<int64_t>( RAYTRACE_STEP_SIZE_IN_CELL_UNITS * Ary * (1L <<INTPRECNUMBIT) );
145 
146  cellType hitCellOcc_int = 0; // p2l(0.5f)
147  const cellType threshold_free_int = p2l(threshold_free);
148  int x, y=int_y2idx(ryi);
149 
150  while ( (x=int_x2idx(rxi))>=0 && (y=int_y2idx(ryi))>=0 &&
151  x<static_cast<int>(size_x) && y<static_cast<int>(size_y) && (hitCellOcc_int=map[x+y*size_x])>threshold_free_int &&
152  ray_len<max_ray_len )
153  {
154  rxi+=Arxi;
155  ryi+=Aryi;
156  ray_len++;
157  }
158 
159  // Store:
160  // Check out of the grid?
161  // Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
162  if (abs(hitCellOcc_int)<=1 || static_cast<unsigned>(x)>=size_x || static_cast<unsigned>(y)>=size_y )
163  {
164  out_valid = false;
165  out_range = max_range_meters;
166  }
167  else
168  { // No: The normal case:
169  out_range = RAYTRACE_STEP_SIZE_IN_CELL_UNITS*ray_len*resolution;
170  out_valid = (ray_len<max_ray_len); // out_range<max_range_meters;
171  // Add additive Gaussian noise:
172  if (noiseStd>0 && out_valid)
173  out_range+= noiseStd*randomGenerator.drawGaussian1D_normalized();
174  }
175 }
176 
177 
179  method(sumUnscented),
180  UT_alpha(0.99), UT_kappa(.0), UT_beta(2.0),
181  MC_samples(10),
182  aperture(M_PIf),
183  rightToLeft(true),
184  maxRange(80.f),
185  nRays(361),
186  rangeNoiseStd(.0f),
187  angleNoiseStd(.0f),
188  decimation(1),
189  threshold(.6f)
190 {
191 }
192 
194 {
195 }
196 
198 {
201 };
202 
203 static void func_laserSimul_callback(const Eigen::Vector3d &x_pose,const TFunctorLaserSimulData &fixed_param, Eigen::VectorXd &y_scanRanges)
204 {
205  ASSERT_(fixed_param.params && fixed_param.grid)
206  ASSERT_(fixed_param.params->decimation>=1)
207  ASSERT_(fixed_param.params->nRays>=2)
208 
209  const size_t N = fixed_param.params->nRays;
210 
211  // Sensor pose in global coordinates
212  const CPose3D sensorPose3D = CPose3D(x_pose[0],x_pose[1],.0, x_pose[2], .0, .0) + 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 = sensorPose.phi() + (fixed_param.params->rightToLeft ? -0.5:+0.5) * fixed_param.params->aperture;
220  const double AA = (fixed_param.params->rightToLeft ? 1.0:-1.0) * (fixed_param.params->aperture / (N-1));
221 
222  const float free_thres = 1.0f-fixed_param.params->threshold;
223 
224  for (size_t i=0;i<N;i+=fixed_param.params->decimation,A+=AA*fixed_param.params->decimation)
225  {
226  bool valid;
227  float range;
228 
229  fixed_param.grid->simulateScanRay(
230  sensorPose.x(),sensorPose.y(),A,
231  range, valid,
232  fixed_param.params->maxRange, free_thres,
233  .0 /*noiseStd*/, .0 /*angleNoiseStd*/ );
234  y_scanRanges[i] = valid ? range : fixed_param.params->maxRange;
235  }
236 }
237 
241 {
242  const Eigen::Vector3d robPoseMean = in_params.robotPose.mean.getAsVectorVal();
243 
244  TFunctorLaserSimulData simulData;
245  simulData.grid = this;
246  simulData.params = &in_params;
247 
248  switch (in_params.method)
249  {
250  case sumUnscented:
252  robPoseMean, // x_mean
253  in_params.robotPose.cov, // x_cov
254  &func_laserSimul_callback, // void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param, VECTORLIKE3 &y)
255  simulData, // const USERPARAM &fixed_param,
256  out_results.scanWithUncert.rangesMean, out_results.scanWithUncert.rangesCovar,
257  NULL, // elem_do_wrap2pi,
258  in_params.UT_alpha, in_params.UT_kappa, in_params.UT_beta // alpha, K, beta
259  );
260  break;
261  case sumMonteCarlo:
262  //
264  robPoseMean, // x_mean
265  in_params.robotPose.cov, // x_cov
266  &func_laserSimul_callback, // void (*functor)(const VECTORLIKE1 &x,const USERPARAM &fixed_param, VECTORLIKE3 &y)
267  simulData, // const USERPARAM &fixed_param,
268  out_results.scanWithUncert.rangesMean, out_results.scanWithUncert.rangesCovar,
269  in_params.MC_samples
270  );
271  break;
272  default:
273  throw std::runtime_error("[laserScanSimulatorWithUncertainty] Unknown `method` value"); break;
274  };
275 
276  // Outputs:
277  out_results.scanWithUncert.rangeScan.aperture = in_params.aperture;
278  out_results.scanWithUncert.rangeScan.maxRange = in_params.maxRange;
279  out_results.scanWithUncert.rangeScan.rightToLeft = in_params.rightToLeft;
280  out_results.scanWithUncert.rangeScan.sensorPose = in_params.sensorPose;
281 
282  out_results.scanWithUncert.rangeScan.resizeScan(in_params.nRays);
283  for (unsigned i=0;i<in_params.nRays;i++) {
284  out_results.scanWithUncert.rangeScan.setScanRange(i, (float)out_results.scanWithUncert.rangesMean[i] );
285  out_results.scanWithUncert.rangeScan.setScanRangeValidity(i, true);
286  }
287 
288  // Add minimum uncertainty: grid cell resolution:
289  out_results.scanWithUncert.rangesCovar.diagonal().array() += 0.5*resolution*resolution;
290 }
291 
const COccupancyGridMap2D::TLaserSimulUncertaintyParams * params
mrpt::poses::CPosePDFGaussian robotPose
The robot pose Gaussian, in map coordinates. Recall that sensor pose relative to this robot pose must...
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:181
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
CPose2D mean
The mean value.
float threshold
(Default: 0.6f) The minimum occupancy threshold to consider a cell to be occupied ...
GLenum GLenum range
Definition: glew.h:7127
void laserScanSimulatorWithUncertainty(const TLaserSimulUncertaintyParams &in_params, TLaserSimulUncertaintyResult &out_results) const
Like laserScanSimulatorWithUncertainty() (see it for a discussion of most parameters) but taking into...
static void func_laserSimul_callback(const Eigen::Vector3d &x_pose, const TFunctorLaserSimulData &fixed_param, Eigen::VectorXd &y_scanRanges)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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...
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)
float resolution
Cell size, i.e. resolution of the grid map.
Output params for laserScanSimulatorWithUncertainty()
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
#define int_y2idx(_Y)
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.
double drawGaussian1D_normalized(double *likelihood=NULL)
Generate a normalized (mean=0, std=1) normally distributed sample.
#define int_x2idx(_X)
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define M_PIf
#define MRPT_END
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, typename mrpt::aligned_containers< VECTORLIKE3 >::vector_t *out_samples_y=NULL)
Simple Montecarlo-base estimation of the Gaussian distribution of a variable Y=f(X) for an arbitrary ...
size_t MC_samples
[sumMonteCarlo] MonteCarlo parameter: number of samples (Default: 10)
__int64 int64_t
Definition: rptypes.h:51
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.
float aperture
(Default: M_PI) The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
double UT_beta
[sumUnscented] UT parameters. Defaults: alpha=0.99, kappa=0, betta=2.0
unsigned int decimation
(Default: 1) The rays that will be simulated are at indexes: 0, D, 2D, 3D,...
#define DEG2RAD
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::utils::DEG2RAD(0)) const
Simulates a laser range scan into the current grid map.
A class for storing an occupancy grid map.
#define MRPT_START
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:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Input params for laserScanSimulatorWithUncertainty()
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
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=NULL, 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...
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)...
#define ASSERT_(f)
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
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...
std::deque< TMeasurement >::iterator iterator
float maxRange
(Default: 80) The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
void sonarSimulator(mrpt::obs::CObservationRange &inout_observation, const mrpt::poses::CPose2D &robotPose, float threshold=0.5f, float rangeNoiseStd=0.f, float angleNoiseStd=mrpt::utils::DEG2RAD(0.f)) const
Simulates the observations of a sonar rig into the current grid map.
int16_t cellType
The type of the map cells:
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.
void setScanRangeValidity(const size_t i, const bool val)
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