MRPT  2.0.4
COccupancyGridMap3D.h
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 #pragma once
10 
14 #include <mrpt/maps/CMetricMap.h>
19 
20 namespace mrpt::maps
21 {
22 /** A 3D occupancy grid map with a regular, even distribution of voxels.
23  *
24  * This is a faster alternative to COctoMap, but use with caution with limited
25  *map extensions, since it could easily exaust available memory.
26  *
27  * Each voxel follows a Bernoulli probability distribution: a value of 0 means
28  *certainly occupied, 1 means a certainly empty voxel. Initially 0.5 means
29  *uncertainty.
30  *
31  * \ingroup mrpt_maps_grp
32  **/
34  : public CMetricMap,
35  public CLogOddsGridMap3D<OccGridCellTraits::cellType>
36 {
38  public:
39  /** The type of the map voxels: */
42 
43  protected:
44  /** Lookup tables for log-odds */
46 
47  /** True upon construction; used by isEmpty() */
48  bool m_is_empty{true};
49 
50  /** See base class */
52 
53  /** Clear the map: It set all voxels to their default occupancy value (0.5),
54  * without changing the resolution (the grid extension is reset to the
55  * default values). */
56  void internal_clear() override;
57 
58  /** Insert the observation information into this map.
59  *
60  * \param obs The observation
61  * \param robotPose The 3D pose of the robot mobile base in the map
62  * reference system, or nullptr (default) if you want to use
63  * CPose2D(0,0,deg)
64  *
65  * \sa insertionOptions, CObservation::insertObservationInto
66  */
68  const mrpt::obs::CObservation& obs,
69  const mrpt::poses::CPose3D* robotPose = nullptr) override;
70 
73  const mrpt::poses::CPose3D& robotPose);
74 
77  const mrpt::poses::CPose3D& robotPose);
78 
79  public:
80  /** Constructor */
82  const mrpt::math::TPoint3D& corner_min = {-5.0f, -5.0f, -5.0f},
83  const mrpt::math::TPoint3D& corner_max = {5.0f, 5.0f, 5.0f},
84  float resolution = 0.25f);
85 
86  /** Fills all the voxels with a default value. */
87  void fill(float default_value = 0.5f);
88 
89  /** Change the size of gridmap, erasing all its previous contents.
90  * \param resolution The new size of voxels.
91  * \param default_value The value of voxels, tipically 0.5.
92  * \sa ResizeGrid
93  */
94  void setSize(
95  const mrpt::math::TPoint3D& corner_min,
96  const mrpt::math::TPoint3D& corner_max, double resolution,
97  float default_value = 0.5f);
98 
99  /** Change the size of gridmap, maintaining previous contents.
100  * \param new_voxels_default_value Value of new voxels, tipically 0.5
101  * \sa setSize()
102  */
103  void resizeGrid(
104  const mrpt::math::TPoint3D& corner_min,
105  const mrpt::math::TPoint3D& corner_max,
106  float new_voxels_default_value = 0.5f);
107 
108  /** Scales an integer representation of the log-odd into a real valued
109  * probability in [0,1], using p=exp(l)/(1+exp(l)) */
110  static inline float l2p(const voxelType l)
111  {
112  return get_logodd_lut().l2p(l);
113  }
114  /** Scales an integer representation of the log-odd into a linear scale
115  * [0,255], using p=exp(l)/(1+exp(l)) */
116  static inline uint8_t l2p_255(const voxelType l)
117  {
118  return get_logodd_lut().l2p_255(l);
119  }
120  /** Scales a real valued probability in [0,1] to an integer representation
121  * of: log(p)-log(1-p) in the valid range of voxelType */
122  static inline voxelType p2l(const float p)
123  {
124  return get_logodd_lut().p2l(p);
125  }
126 
127  /** Performs the Bayesian fusion of a new observation of a cell \sa
128  * updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free */
129  void updateCell(int cx_idx, int cy_idx, int cz_idx, float v);
130 
131  /** Change the contents [0,1] (0:occupied, 1:free) of a voxel, given its
132  * index. */
133  inline void setCellFreeness(
134  unsigned int cx, unsigned int cy, unsigned int cz, float value)
135  {
136  if (auto* c = m_grid.cellByIndex(cx, cy, cz); c != nullptr)
137  *c = p2l(value);
138  }
139 
140  /** Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel,
141  * given its index */
142  inline float getCellFreeness(
143  unsigned int cx, unsigned int cy, unsigned int cz) const
144  {
145  if (auto* c = m_grid.cellByIndex(cx, cy, cz); c != nullptr)
146  return l2p(*c);
147  else
148  return .5f;
149  }
150 
151  /** Change the contents [0,1] of a voxel, given its coordinates */
152  inline void setFreenessByPos(float x, float y, float z, float value)
153  {
155  m_grid.x2idx(x), m_grid.y2idx(y), m_grid.z2idx(z), value);
156  }
157 
158  /** Read the real valued [0,1] contents of a voxel, given its coordinates */
159  inline float getFreenessByPos(float x, float y, float z) const
160  {
161  return getCellFreeness(
162  m_grid.x2idx(x), m_grid.y2idx(y), m_grid.z2idx(z));
163  }
164 
165  /** Increases the freeness of a ray segment, and the occupancy of the voxel
166  * at its end point (unless endIsOccupied=false).
167  * Normally, users would prefer the higher-level method
168  * CMetricMap::insertObservation()
169  */
170  void insertRay(
171  const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end,
172  bool endIsOccupied = true);
173 
174  /** Calls insertRay() for each point in the point cloud, using as sensor
175  * central point (the origin of all rays), the given `sensorCenter`.
176  * \param[in] maxValidRange If a point has larger distance from
177  * `sensorCenter` than `maxValidRange`, it will be considered a non-echo,
178  * and NO occupied voxel will be created at the end of the segment.
179  * \sa insertionOptions parameters are observed in this method.
180  */
181  void insertPointCloud(
182  const mrpt::math::TPoint3D& sensorCenter,
183  const mrpt::maps::CPointsMap& pts,
184  const float maxValidRange = std::numeric_limits<float>::max());
185 
186  /** \sa renderingOptions */
188 
189  /** Returns a 3D object representing the map. \sa renderingOptions */
190  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
191 
192  /** With this struct options are provided to the observation insertion
193  * process.
194  * \sa CObservation::insertIntoGridMap */
196  {
197  public:
198  TInsertionOptions() = default;
199 
200  /** This method load the options from a ".ini" file.
201  * Only those parameters found in the given "section" and having
202  * the same name that the variable are loaded. Those not found in
203  * the file will stay with their previous values (usually the default
204  * values loaded at initialization). An example of an ".ini" file:
205  * \code
206  * [section]
207  * resolution=0.10 ; blah blah...
208  * modeSelection=1 ; 0=blah, 1=blah,...
209  * \endcode
210  */
211  void loadFromConfigFile(
212  const mrpt::config::CConfigFileBase& source,
213  const std::string& section) override;
214  // See base docs
215  void saveToConfigFile(
217  const std::string& section) const override;
218 
219  /** Largest distance at which voxels are updated (Default: 15 meters) */
220  float maxDistanceInsertion{15.0f};
221  /** A value in the range [0.5,1] used for updating voxel with a Bayesian
222  * approach (default 0.65) */
224  /** A value in the range [0.5,1] for updating a free voxel. (default=0
225  * means use the same than maxOccupancyUpdateCertainty) */
227  /** Specify the decimation of 3D range scans.
228  * "N" means keeping the minimum range of each block of "NxN" range
229  * pixels.
230  * (default=8)
231  */
232  uint16_t decimation_3d_range{8};
233 
234  /** Decimation for insertPointCloud() or 2D range scans (Default: 1) */
235  uint16_t decimation{1};
236  };
237 
238  /** With this struct options are provided to the observation insertion
239  * process \sa CObservation::insertIntoGridMap */
241 
242  /** The type for selecting a likelihood computation method */
244  {
247  // Remember: Update TEnumType below if new values are added here!
248  };
249 
250  /** With this struct options are provided to the observation likelihood
251  * computation process */
253  {
254  public:
255  TLikelihoodOptions() = default;
256 
257  /** This method load the options from a ".ini" file.
258  * Only those parameters found in the given "section" and having
259  * the same name that the variable are loaded. Those not found in
260  * the file will stay with their previous values (usually the default
261  * values loaded at initialization). An example of an ".ini" file:
262  * \code
263  * [section]
264  * resolution=0.10 ; blah blah...
265  * modeSelection=1 ; 0=blah, 1=blah,...
266  * \endcode
267  */
268  void loadFromConfigFile(
269  const mrpt::config::CConfigFileBase& source,
270  const std::string& section) override;
271  // See base docs
272  void saveToConfigFile(
274  const std::string& s) const override;
275 
276  /** The selected method to compute an observation likelihood */
278  /** [LikelihoodField] The laser range "sigma" used in computations;
279  * Default value = 0.35 */
280  float LF_stdHit{0.35f};
281  /** [LikelihoodField] */
282  float LF_zHit{0.95f}, LF_zRandom{0.05f};
283  /** [LikelihoodField] The max. range of the sensor (Default= 81 m) */
284  float LF_maxRange{20.0f};
285  /** [LikelihoodField] The decimation of the points in a scan, default=1
286  * == no decimation */
287  uint32_t LF_decimation{1};
288  /** [LikelihoodField] The max. distance for searching correspondences
289  * around each sensed point */
290  float LF_maxCorrsDistance{0.3f};
291  /** [LikelihoodField] (Default:false) Use `exp(dist^2/std^2)` instead of
292  * `exp(dist^2/std^2)` */
293  bool LF_useSquareDist{false};
294  /** [rayTracing] One out of "rayTracing_decimation" rays will be
295  * simulated and compared only: set to 1 to use all the sensed ranges.
296  */
298  /** [rayTracing] The laser range sigma. */
299  float rayTracing_stdHit{1.0f};
300  };
301 
303 
304  /** Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a
305  * mrpt::opengl::COctoMapVoxels */
307  {
308  TRenderingOptions() = default;
309 
310  /** Generate grid lines for all octree nodes, useful to draw the
311  "structure" of the
312  octree, but computationally costly (Default: false) */
313  bool generateGridLines{false};
314 
315  /** Generate voxels for the occupied volumes (Default=true) */
317 
318  /** Set occupied voxels visible (requires generateOccupiedVoxels=true)
319  * (Default=true)*/
321 
322  /** Generate voxels for the freespace (Default=true) */
323  bool generateFreeVoxels{true};
324 
325  /** Set free voxels visible (requires generateFreeVoxels=true)
326  * (Default=true) */
327  bool visibleFreeVoxels{true};
328 
329  /** Binary dump to stream */
331  /** Binary dump to stream */
333  };
334 
336 
337  /** Simulate just one "ray" in the grid map. This method is used internally
338  * to sonarSimulator and laserScanSimulator. \sa
339  * COccupancyGridMap3D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS */
340  void simulateScanRay(
341  const double x, const double y, const double angle_direction,
342  float& out_range, bool& out_valid, const double max_range_meters,
343  const float threshold_free = 0.4f, const double noiseStd = .0,
344  const double angleNoiseStd = .0) const;
345 
346  /** Computes the likelihood [0,1] of a set of points, given the current grid
347  * map as reference.
348  * \param pm The points map
349  * \param relativePose The relative pose of the points map in this map's
350  * coordinates, or nullptr for (0,0,0).
351  * See "likelihoodOptions" for configuration parameters.
352  */
354  const CPointsMap& pm,
355  const mrpt::poses::CPose3D& relativePose = mrpt::poses::CPose3D());
356 
357  /** Returns true upon map construction or after calling clear(), the return
358  * changes to false upon successful insertObservation() or any other
359  * method to load data in the map.
360  */
361  bool isEmpty() const override;
362 
363  /** See docs in base class: in this class this always returns 0 */
364  void determineMatching2D(
365  const mrpt::maps::CMetricMap* otherMap,
366  const mrpt::poses::CPose2D& otherMapPose,
367  mrpt::tfest::TMatchingPairList& correspondences,
368  const TMatchingParams& params,
369  TMatchingExtraResults& extraResults) const override;
370 
371  /** See docs in base class: in this class this always returns 0 */
373  const mrpt::maps::CMetricMap* otherMap,
374  const mrpt::poses::CPose3D& otherMapPose,
375  const TMatchingRatioParams& params) const override;
376 
377  void saveMetricMapRepresentationToFile(const std::string& f) const override;
378 
379  private:
380  // See docs in base class
382  const mrpt::obs::CObservation& obs,
383  const mrpt::poses::CPose3D& takenFrom) override;
384  // See docs in base class
386  const mrpt::obs::CObservation& obs) const override;
387 
389  /** See COccupancyGridMap3D::COccupancyGridMap3D */
390  float min_x{-5.0f}, max_x{5.0f};
391  float min_y{-5.0f}, max_y{5.0f};
392  float min_z{-5.0f}, max_z{5.0f};
393  float resolution{0.25f};
394 
395  /** Observations insertion options */
397  /** Probabilistic observation likelihood options */
400 };
401 
402 } // namespace mrpt::maps
403 
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation &obs) const override
Parameters for CMetricMap::compute3DMatchingRatio()
void setCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz, float value)
Change the contents [0,1] (0:occupied, 1:free) of a voxel, given its index.
OccGridCellTraits::cellTypeUnsigned voxelTypeUnsigned
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
MRPT_FILL_ENUM_MEMBER(mrpt::maps::COccupancyGridMap3D, lmRayTracing)
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation ...
void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
See docs in base class: in this class this always returns 0.
With this struct options are provided to the observation insertion process.
void saveToConfigFile(mrpt::config::CConfigFileBase &target, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const
Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel, given its index.
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
static CLogOddsGridMapLUT< voxelType > & get_logodd_lut()
Lookup tables for log-odds.
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
double computeLikelihoodField_Thrun(const CPointsMap &pm, const mrpt::poses::CPose3D &relativePose=mrpt::poses::CPose3D())
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
mrpt::vision::TStereoCalibParams params
void internal_insertObservationScan2D(const mrpt::obs::CObservation2DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
float getFreenessByPos(float x, float y, float z) const
Read the real valued [0,1] contents of a voxel, given its coordinates.
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
mrpt::maps::COccupancyGridMap3D::TInsertionOptions insertionOpts
Observations insertion options.
uint16_t decimation_3d_range
Specify the decimation of 3D range scans.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
void setSize(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, double resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2) ...
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:67
This class allows loading and storing values and vectors of different types from a configuration text...
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Insert the observation information into this map.
static uint8_t l2p_255(const voxelType l)
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)...
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
void updateCell(int cx_idx, int cy_idx, int cz_idx, float v)
Performs the Bayesian fusion of a new observation of a cell.
With this struct options are provided to the observation likelihood computation process.
float maxDistanceInsertion
Largest distance at which voxels are updated (Default: 15 meters)
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
int x2idx(coord_t x) const
Transform a coordinate values into voxel indexes.
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
void insertRay(const mrpt::math::TPoint3D &sensor, const mrpt::math::TPoint3D &end, bool endIsOccupied=true)
Increases the freeness of a ray segment, and the occupancy of the voxel at its end point (unless endI...
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true) ...
void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation &) override
See base class.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
See docs in base class: in this class this always returns 0.
void internal_clear() override
Clear the map: It set all voxels to their default occupancy value (0.5), without changing the resolut...
T * cellByIndex(unsigned int cx, unsigned int cy, unsigned int cz)
Returns a pointer to the contents of a voxel given by its voxel indexes, or nullptr if it is out of t...
bool m_is_empty
True upon construction; used by isEmpty()
void resizeGrid(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, float new_voxels_default_value=0.5f)
Change the size of gridmap, maintaining previous contents.
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
static float l2p(const voxelType l)
Scales an integer representation of the log-odd into a real valued probability in [0...
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free voxel.
A 3D occupancy grid map with a regular, even distribution of voxels.
void internal_insertObservationScan3D(const mrpt::obs::CObservation3DRangeScan &o, const mrpt::poses::CPose3D &robotPose)
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
const_iterator end() const
Definition: ts_hash_map.h:246
void setFreenessByPos(float x, float y, float z, float value)
Change the contents [0,1] of a voxel, given its coordinates.
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
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.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
TLikelihoodMethod
The type for selecting a likelihood computation method.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
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
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void fill(float default_value=0.5f)
Fills all the voxels with a default value.
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0...
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
void insertPointCloud(const mrpt::math::TPoint3D &sensorCenter, const mrpt::maps::CPointsMap &pts, const float maxValidRange=std::numeric_limits< float >::max())
Calls insertRay() for each point in the point cloud, using as sensor central point (the origin of all...
COccupancyGridMap3D(const mrpt::math::TPoint3D &corner_min={-5.0f, -5.0f, -5.0f}, const mrpt::math::TPoint3D &corner_max={5.0f, 5.0f, 5.0f}, float resolution=0.25f)
Constructor.
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
A generic provider of log-odds grid-map maintainance functions.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
static voxelType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
void saveMetricMapRepresentationToFile(const std::string &f) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
Parameters for the determination of matchings between point clouds, etc.
float rayTracing_stdHit
[rayTracing] The laser range sigma.
#define MAP_DEFINITION_END(_CLASS_NAME_)
Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a mrpt::opengl::COctoMapVoxels.
OccGridCellTraits::cellType voxelType
The type of the map voxels:
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 3e8ecd670 Mon May 18 14:23:19 2020 +0200 at lun may 18 14:30:11 CEST 2020