MRPT  1.9.9
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-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 #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  // friend class CMultiMetricMap;
45  // friend class CMultiMetricMapPDF;
46 
47  /** Lookup tables for log-odds */
49 
50  /** True upon construction; used by isEmpty() */
51  bool m_is_empty{true};
52 
53  /** See base class */
55 
56  /** Clear the map: It set all voxels to their default occupancy value (0.5),
57  * without changing the resolution (the grid extension is reset to the
58  * default values). */
59  void internal_clear() override;
60 
61  /** Insert the observation information into this map.
62  *
63  * \param obs The observation
64  * \param robotPose The 3D pose of the robot mobile base in the map
65  * reference system, or nullptr (default) if you want to use
66  * CPose2D(0,0,deg)
67  *
68  * \sa insertionOptions, CObservation::insertObservationInto
69  */
71  const mrpt::obs::CObservation& obs,
72  const mrpt::poses::CPose3D* robotPose = nullptr) override;
73 
76  const mrpt::poses::CPose3D& robotPose);
77 
80  const mrpt::poses::CPose3D& robotPose);
81 
82  public:
83  /** Constructor */
85  const mrpt::math::TPoint3D& corner_min = {-5.0f, -5.0f, -5.0f},
86  const mrpt::math::TPoint3D& corner_max = {5.0f, 5.0f, 5.0f},
87  float resolution = 0.25f);
88 
89  /** Fills all the voxels with a default value. */
90  void fill(float default_value = 0.5f);
91 
92  /** Change the size of gridmap, erasing all its previous contents.
93  * \param resolution The new size of voxels.
94  * \param default_value The value of voxels, tipically 0.5.
95  * \sa ResizeGrid
96  */
97  void setSize(
98  const mrpt::math::TPoint3D& corner_min,
99  const mrpt::math::TPoint3D& corner_max, float resolution,
100  float default_value = 0.5f);
101 
102  /** Change the size of gridmap, maintaining previous contents.
103  * \param new_voxels_default_value Value of new voxels, tipically 0.5
104  * \sa setSize()
105  */
106  void resizeGrid(
107  const mrpt::math::TPoint3D& corner_min,
108  const mrpt::math::TPoint3D& corner_max,
109  float new_voxels_default_value = 0.5f);
110 
111  /** Scales an integer representation of the log-odd into a real valued
112  * probability in [0,1], using p=exp(l)/(1+exp(l)) */
113  static inline float l2p(const voxelType l)
114  {
115  return get_logodd_lut().l2p(l);
116  }
117  /** Scales an integer representation of the log-odd into a linear scale
118  * [0,255], using p=exp(l)/(1+exp(l)) */
119  static inline uint8_t l2p_255(const voxelType l)
120  {
121  return get_logodd_lut().l2p_255(l);
122  }
123  /** Scales a real valued probability in [0,1] to an integer representation
124  * of: log(p)-log(1-p) in the valid range of voxelType */
125  static inline voxelType p2l(const float p)
126  {
127  return get_logodd_lut().p2l(p);
128  }
129 
130  /** Performs the Bayesian fusion of a new observation of a cell \sa
131  * updateInfoChangeOnly, updateCell_fast_occupied, updateCell_fast_free */
132  void updateCell(int cx_idx, int cy_idx, int cz_idx, float v);
133 
134  /** Change the contents [0,1] (0:occupied, 1:free) of a voxel, given its
135  * index. */
136  inline void setCellFreeness(
137  unsigned int cx, unsigned int cy, unsigned int cz, float value)
138  {
139  if (auto* c = m_grid.cellByIndex(cx, cy, cz); c != nullptr)
140  *c = p2l(value);
141  }
142 
143  /** Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel,
144  * given its index */
145  inline float getCellFreeness(
146  unsigned int cx, unsigned int cy, unsigned int cz) const
147  {
148  if (auto* c = m_grid.cellByIndex(cx, cy, cz); c != nullptr)
149  return l2p(*c);
150  else
151  return .5f;
152  }
153 
154  /** Change the contents [0,1] of a voxel, given its coordinates */
155  inline void setFreenessByPos(float x, float y, float z, float value)
156  {
158  m_grid.x2idx(x), m_grid.y2idx(y), m_grid.z2idx(z), value);
159  }
160 
161  /** Read the real valued [0,1] contents of a voxel, given its coordinates */
162  inline float getFreenessByPos(float x, float y, float z) const
163  {
164  return getCellFreeness(
165  m_grid.x2idx(x), m_grid.y2idx(y), m_grid.z2idx(z));
166  }
167 
168  /** Increases the freeness of a ray segment, and the occupancy of the voxel
169  * at its end point (unless endIsOccupied=false).
170  * Normally, users would prefer the higher-level method
171  * CMetricMap::insertObservation()
172  */
173  void insertRay(
174  const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end,
175  bool endIsOccupied = true);
176 
177  /** Calls insertRay() for each point in the point cloud, using as sensor
178  * central point (the origin of all rays), the given `sensorCenter`.
179  * \param[in] maxValidRange If a point has larger distance from
180  * `sensorCenter` than `maxValidRange`, it will be considered a non-echo,
181  * and NO occupied voxel will be created at the end of the segment.
182  * \sa insertionOptions parameters are observed in this method.
183  */
184  void insertPointCloud(
185  const mrpt::math::TPoint3D& sensorCenter,
186  const mrpt::maps::CPointsMap& pts,
187  const float maxValidRange = std::numeric_limits<float>::max());
188 
189  /** \sa renderingOptions */
191 
192  /** Returns a 3D object representing the map. \sa renderingOptions */
193  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
194 
195  /** With this struct options are provided to the observation insertion
196  * process.
197  * \sa CObservation::insertIntoGridMap */
199  {
200  public:
201  TInsertionOptions() = default;
202 
203  /** This method load the options from a ".ini" file.
204  * Only those parameters found in the given "section" and having
205  * the same name that the variable are loaded. Those not found in
206  * the file will stay with their previous values (usually the default
207  * values loaded at initialization). An example of an ".ini" file:
208  * \code
209  * [section]
210  * resolution=0.10 ; blah blah...
211  * modeSelection=1 ; 0=blah, 1=blah,...
212  * \endcode
213  */
214  void loadFromConfigFile(
215  const mrpt::config::CConfigFileBase& source,
216  const std::string& section) override;
217  // See base docs
218  void saveToConfigFile(
220  const std::string& section) const override;
221 
222  /** Largest distance at which voxels are updated (Default: 15 meters) */
223  float maxDistanceInsertion{15.0f};
224  /** A value in the range [0.5,1] used for updating voxel with a Bayesian
225  * approach (default 0.65) */
227  /** A value in the range [0.5,1] for updating a free voxel. (default=0
228  * means use the same than maxOccupancyUpdateCertainty) */
230  /** Specify the decimation of 3D range scans.
231  * "N" means keeping the minimum range of each block of "NxN" range
232  * pixels.
233  * (default=8)
234  */
235  uint16_t decimation_3d_range{8};
236 
237  /** Decimation for insertPointCloud() or 2D range scans (Default: 1) */
238  uint16_t decimation{1};
239  };
240 
241  /** With this struct options are provided to the observation insertion
242  * process \sa CObservation::insertIntoGridMap */
244 
245  /** The type for selecting a likelihood computation method */
247  {
250  // Remember: Update TEnumType below if new values are added here!
251  };
252 
253  /** With this struct options are provided to the observation likelihood
254  * computation process */
256  {
257  public:
258  TLikelihoodOptions() = default;
259 
260  /** This method load the options from a ".ini" file.
261  * Only those parameters found in the given "section" and having
262  * the same name that the variable are loaded. Those not found in
263  * the file will stay with their previous values (usually the default
264  * values loaded at initialization). An example of an ".ini" file:
265  * \code
266  * [section]
267  * resolution=0.10 ; blah blah...
268  * modeSelection=1 ; 0=blah, 1=blah,...
269  * \endcode
270  */
271  void loadFromConfigFile(
272  const mrpt::config::CConfigFileBase& source,
273  const std::string& section) override;
274  // See base docs
275  void saveToConfigFile(
277  const std::string& s) const override;
278 
279  /** The selected method to compute an observation likelihood */
281  /** [LikelihoodField] The laser range "sigma" used in computations;
282  * Default value = 0.35 */
283  float LF_stdHit{0.35f};
284  /** [LikelihoodField] */
285  float LF_zHit{0.95f}, LF_zRandom{0.05f};
286  /** [LikelihoodField] The max. range of the sensor (Default= 81 m) */
287  float LF_maxRange{20.0f};
288  /** [LikelihoodField] The decimation of the points in a scan, default=1
289  * == no decimation */
290  uint32_t LF_decimation{1};
291  /** [LikelihoodField] The max. distance for searching correspondences
292  * around each sensed point */
293  float LF_maxCorrsDistance{0.3f};
294  /** [LikelihoodField] (Default:false) Use `exp(dist^2/std^2)` instead of
295  * `exp(dist^2/std^2)` */
296  bool LF_useSquareDist{false};
297  /** [rayTracing] One out of "rayTracing_decimation" rays will be
298  * simulated and compared only: set to 1 to use all the sensed ranges.
299  */
301  /** [rayTracing] The laser range sigma. */
302  float rayTracing_stdHit{1.0f};
303  };
304 
306 
307  /** Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a
308  * mrpt::opengl::COctoMapVoxels */
310  {
311  TRenderingOptions() = default;
312 
313  /** Generate grid lines for all octree nodes, useful to draw the
314  "structure" of the
315  octree, but computationally costly (Default: false) */
316  bool generateGridLines{false};
317 
318  /** Generate voxels for the occupied volumes (Default=true) */
320 
321  /** Set occupied voxels visible (requires generateOccupiedVoxels=true)
322  * (Default=true)*/
324 
325  /** Generate voxels for the freespace (Default=true) */
326  bool generateFreeVoxels{true};
327 
328  /** Set free voxels visible (requires generateFreeVoxels=true)
329  * (Default=true) */
330  bool visibleFreeVoxels{true};
331 
332  /** Binary dump to stream */
334  /** Binary dump to stream */
336  };
337 
339 
340  /** Simulate just one "ray" in the grid map. This method is used internally
341  * to sonarSimulator and laserScanSimulator. \sa
342  * COccupancyGridMap3D::RAYTRACE_STEP_SIZE_IN_CELL_UNITS */
343  void simulateScanRay(
344  const double x, const double y, const double angle_direction,
345  float& out_range, bool& out_valid, const double max_range_meters,
346  const float threshold_free = 0.4f, const double noiseStd = .0,
347  const double angleNoiseStd = .0) const;
348 
349  /** Computes the likelihood [0,1] of a set of points, given the current grid
350  * map as reference.
351  * \param pm The points map
352  * \param relativePose The relative pose of the points map in this map's
353  * coordinates, or nullptr for (0,0,0).
354  * See "likelihoodOptions" for configuration parameters.
355  */
357  const CPointsMap& pm,
358  const mrpt::poses::CPose3D& relativePose = mrpt::poses::CPose3D());
359 
360  /** Returns true upon map construction or after calling clear(), the return
361  * changes to false upon successful insertObservation() or any other
362  * method to load data in the map.
363  */
364  bool isEmpty() const override;
365 
366  /** See docs in base class: in this class this always returns 0 */
367  void determineMatching2D(
368  const mrpt::maps::CMetricMap* otherMap,
369  const mrpt::poses::CPose2D& otherMapPose,
370  mrpt::tfest::TMatchingPairList& correspondences,
371  const TMatchingParams& params,
372  TMatchingExtraResults& extraResults) const override;
373 
374  /** See docs in base class: in this class this always returns 0 */
376  const mrpt::maps::CMetricMap* otherMap,
377  const mrpt::poses::CPose3D& otherMapPose,
378  const TMatchingRatioParams& params) const override;
379 
380  void saveMetricMapRepresentationToFile(const std::string& f) const override;
381 
382  private:
383  // See docs in base class
385  const mrpt::obs::CObservation& obs,
386  const mrpt::poses::CPose3D& takenFrom) override;
387  // See docs in base class
389  const mrpt::obs::CObservation& obs) const override;
390 
392  /** See COccupancyGridMap3D::COccupancyGridMap3D */
393  float min_x{-5.0f}, max_x{5.0f};
394  float min_y{-5.0f}, max_y{5.0f};
395  float min_z{-5.0f}, max_z{5.0f};
396  float resolution{0.25f};
397 
398  /** Observations insertion options */
400  /** Probabilistic observation likelihood options */
403 };
404 
405 } // namespace mrpt::maps
406 
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation &obs) const override
Internal method called by canComputeObservationLikelihood()
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)
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
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.
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:65
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 >...
Lightweight 3D point.
Definition: TPoint3D.h:90
#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.
void setSize(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
#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 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019