MRPT  2.0.5
COctoMapBase.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 
10 #pragma once
11 
13 #include <mrpt/core/pimpl.h>
15 #include <mrpt/maps/CMetricMap.h>
16 #include <mrpt/obs/obs_frwds.h>
19 
20 namespace mrpt::maps
21 {
22 /** A three-dimensional probabilistic occupancy grid, implemented as an
23  * octo-tree with the "octomap" C++ library.
24  * This base class represents a 3D map where each voxel may contain an
25  * "occupancy" property, RGBD data, etc. depending on the derived class.
26  *
27  * As with any other mrpt::maps::CMetricMap, you can obtain a 3D representation
28  * of the map calling getAs3DObject() or getAsOctoMapVoxels()
29  *
30  * To use octomap's iterators to go through the voxels, use
31  * COctoMap::getOctomap()
32  *
33  * The octomap library was presented in:
34  * - K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard,
35  * <i>"OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation
36  * for Robotic Systems"</i>
37  * in Proc. of the ICRA 2010 Workshop on Best Practice in 3D Perception and
38  * Modeling for Mobile Manipulation, 2010. Software available at
39  * http://octomap.sf.net/.
40  *
41  * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
42  * \ingroup mrpt_maps_grp
43  */
44 template <class octree_t, class octree_node_t>
46 {
47  public:
49 
50  /** Constructor, defines the resolution of the octomap (length of each voxel
51  * side) */
52  COctoMapBase(double resolution);
53  ~COctoMapBase() override = default;
54  /** Get a reference to the internal octomap object. Example:
55  * \code
56  * mrpt::maps::COctoMap map;
57  * octomap::OcTree &om = map.getOctomap<octomap::OcTree>();
58  * \endcode
59  */
60  template <class OCTOMAP_CLASS>
61  inline OCTOMAP_CLASS& getOctomap()
62  {
63  return m_impl->m_octomap;
64  }
65 
66  /** With this struct options are provided to the observation insertion
67  * process.
68  * \sa CObservation::insertObservationInto()
69  */
71  {
72  /** Initilization of default parameters */
73  TInsertionOptions(myself_t& parent);
74 
75  /** Special constructor, not attached to a real COctoMap object: used
76  * only in limited situations, since get*() methods don't work, etc. */
79 
81  {
82  // Copy all but the m_parent pointer!
83  maxrange = o.maxrange;
84  pruning = o.pruning;
85  const bool o_has_parent = o.m_parent.get() != nullptr;
87  o_has_parent ? o.getOccupancyThres() : o.occupancyThres);
88  setProbHit(o_has_parent ? o.getProbHit() : o.probHit);
89  setProbMiss(o_has_parent ? o.getProbMiss() : o.probMiss);
91  o_has_parent ? o.getClampingThresMin() : o.clampingThresMin);
93  o_has_parent ? o.getClampingThresMax() : o.clampingThresMax);
94  return *this;
95  }
96 
97  void loadFromConfigFile(
98  const mrpt::config::CConfigFileBase& source,
99  const std::string& section) override; // See base docs
100  void dumpToTextStream(
101  std::ostream& out) const override; // See base docs
102 
103  double maxrange{
104  -1.}; //!< maximum range for how long individual beams are
105  //! inserted (default -1: complete beam)
106  bool pruning{true}; //!< whether the tree is (losslessly) pruned after
107  //! insertion (default: true)
108 
109  /// (key name in .ini files: "occupancyThres") sets the threshold for
110  /// occupancy (sensor model) (Default=0.5)
111  void setOccupancyThres(double prob)
112  {
113  if (m_parent.get()) m_parent->setOccupancyThres(prob);
114  }
115  /// (key name in .ini files: "probHit")sets the probablility for a "hit"
116  /// (will be converted to logodds) - sensor model (Default=0.7)
117  void setProbHit(double prob)
118  {
119  if (m_parent.get()) m_parent->setProbHit(prob);
120  }
121  /// (key name in .ini files: "probMiss")sets the probablility for a
122  /// "miss" (will be converted to logodds) - sensor model (Default=0.4)
123  void setProbMiss(double prob)
124  {
125  if (m_parent.get()) m_parent->setProbMiss(prob);
126  }
127  /// (key name in .ini files: "clampingThresMin")sets the minimum
128  /// threshold for occupancy clamping (sensor model) (Default=0.1192, -2
129  /// in log odds)
130  void setClampingThresMin(double thresProb)
131  {
132  if (m_parent.get()) m_parent->setClampingThresMin(thresProb);
133  }
134  /// (key name in .ini files: "clampingThresMax")sets the maximum
135  /// threshold for occupancy clamping (sensor model) (Default=0.971, 3.5
136  /// in log odds)
137  void setClampingThresMax(double thresProb)
138  {
139  if (m_parent.get()) m_parent->setClampingThresMax(thresProb);
140  }
141 
142  /// @return threshold (probability) for occupancy - sensor model
143  double getOccupancyThres() const
144  {
145  if (m_parent.get())
146  return m_parent->getOccupancyThres();
147  else
148  return this->occupancyThres;
149  }
150  /// @return threshold (logodds) for occupancy - sensor model
151  float getOccupancyThresLog() const
152  {
153  return m_parent->getOccupancyThresLog();
154  }
155 
156  /// @return probablility for a "hit" in the sensor model (probability)
157  double getProbHit() const
158  {
159  if (m_parent.get())
160  return m_parent->getProbHit();
161  else
162  return this->probHit;
163  }
164  /// @return probablility for a "hit" in the sensor model (logodds)
165  float getProbHitLog() const { return m_parent->getProbHitLog(); }
166  /// @return probablility for a "miss" in the sensor model (probability)
167  double getProbMiss() const
168  {
169  if (m_parent.get())
170  return m_parent->getProbMiss();
171  else
172  return this->probMiss;
173  }
174  /// @return probablility for a "miss" in the sensor model (logodds)
175  float getProbMissLog() const { return m_parent->getProbMissLog(); }
176  /// @return minimum threshold for occupancy clamping in the sensor model
177  /// (probability)
178  double getClampingThresMin() const
179  {
180  if (m_parent.get())
181  return m_parent->getClampingThresMin();
182  else
183  return this->clampingThresMin;
184  }
185  /// @return minimum threshold for occupancy clamping in the sensor model
186  /// (logodds)
188  {
189  return m_parent->getClampingThresMinLog();
190  }
191  /// @return maximum threshold for occupancy clamping in the sensor model
192  /// (probability)
193  double getClampingThresMax() const
194  {
195  if (m_parent.get())
196  return m_parent->getClampingThresMax();
197  else
198  return this->clampingThresMax;
199  }
200  /// @return maximum threshold for occupancy clamping in the sensor model
201  /// (logodds)
203  {
204  return m_parent->getClampingThresMaxLog();
205  }
206 
207  private:
209 
210  double occupancyThres{0.5}; // sets the threshold for occupancy (sensor
211  // model) (Default=0.5)
212  double probHit{
213  0.7}; // sets the probablility for a "hit" (will be converted
214  // to logodds) - sensor model (Default=0.7)
215  double probMiss{0.4}; // sets the probablility for a "miss" (will be
216  // converted to logodds) - sensor model (Default=0.4)
218  0.1192}; // sets the minimum threshold for occupancy
219  // clamping (sensor model) (Default=0.1192, -2
220  // in log odds)
222  0.971}; // sets the maximum threshold for occupancy
223  // clamping (sensor model) (Default=0.971, 3.5
224  // in log odds)
225  };
226 
227  TInsertionOptions insertionOptions; //!< The options used when inserting
228  //! observations in the map
229 
230  /** Options used when evaluating "computeObservationLikelihood"
231  * \sa CObservation::computeObservationLikelihood
232  */
234  {
235  /** Initilization of default parameters
236  */
238  ~TLikelihoodOptions() override = default;
239  void loadFromConfigFile(
240  const mrpt::config::CConfigFileBase& source,
241  const std::string& section) override; // See base docs
242  void dumpToTextStream(
243  std::ostream& out) const override; // See base docs
244 
245  /** Binary dump to stream */
247  /** Binary dump to stream */
249 
250  uint32_t decimation{1}; //!< Speed up the likelihood computation by
251  //! considering only one out of N rays (default=1)
252  };
253 
254  TLikelihoodOptions likelihoodOptions;
255 
257  const std::string& filNamePrefix) const override;
258 
259  /** Options for the conversion of a mrpt::maps::COctoMap into a
260  * mrpt::opengl::COctoMapVoxels */
262  {
264  false}; //!< Generate grid lines for all octree nodes,
265  //! useful to draw the "structure" of the
266  //! octree, but computationally costly (Default:
267  //! false)
268 
270  true}; //!< Generate voxels for the occupied
271  //! volumes (Default=true)
273  true}; //!< Set occupied voxels visible (requires
274  //! generateOccupiedVoxels=true)
275  //!(Default=true)
276 
277  bool generateFreeVoxels{true}; //!< Generate voxels for the freespace
278  //!(Default=true)
279  bool visibleFreeVoxels{true}; //!< Set free voxels visible (requires
280  //! generateFreeVoxels=true) (Default=true)
281 
282  TRenderingOptions() = default;
283 
284  /** Binary dump to stream */
286  /** Binary dump to stream */
288  };
289 
290  TRenderingOptions renderingOptions;
291 
292  /** Returns a 3D object representing the map.
293  * \sa renderingOptions
294  */
295  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override
296  {
297  auto gl_obj = mrpt::opengl::COctoMapVoxels::Create();
298  this->getAsOctoMapVoxels(*gl_obj);
299  outObj->insert(gl_obj);
300  }
301 
302  /** Builds a renderizable representation of the octomap as a
303  * mrpt::opengl::COctoMapVoxels object.
304  * \sa renderingOptions
305  */
306  virtual void getAsOctoMapVoxels(
307  mrpt::opengl::COctoMapVoxels& gl_obj) const = 0;
308 
309  /** Get the occupancy probability [0,1] of a point
310  * \return false if the point is not mapped, in which case the returned
311  * "prob" is undefined. */
312  bool getPointOccupancy(
313  const float x, const float y, const float z,
314  double& prob_occupancy) const;
315 
316  /** Update the octomap with a 2D or 3D scan, given directly as a point cloud
317  * and the 3D location of the sensor (the origin of the rays) in this map's
318  * frame of reference.
319  * Insertion parameters can be found in \a insertionOptions.
320  * \sa The generic observation insertion method
321  * CMetricMap::insertObservation()
322  */
323  void insertPointCloud(
324  const CPointsMap& ptMap, const float sensor_x, const float sensor_y,
325  const float sensor_z);
326 
327  /** Performs raycasting in 3d, similar to computeRay().
328  *
329  * A ray is cast from origin with a given direction, the first occupied
330  * cell is returned (as center coordinate). If the starting coordinate is
331  * already
332  * occupied in the tree, this coordinate will be returned as a hit.
333  *
334  * @param origin starting coordinate of ray
335  * @param direction A vector pointing in the direction of the raycast. Does
336  * not need to be normalized.
337  * @param end returns the center of the cell that was hit by the ray, if
338  * successful
339  * @param ignoreUnknownCells whether unknown cells are ignored. If false
340  * (default), the raycast aborts when an unkown cell is hit.
341  * @param maxRange Maximum range after which the raycast is aborted (<= 0:
342  * no limit, default)
343  * @return whether or not an occupied cell was hit
344  */
345  bool castRay(
346  const mrpt::math::TPoint3D& origin,
347  const mrpt::math::TPoint3D& direction, mrpt::math::TPoint3D& end,
348  bool ignoreUnknownCells = false, double maxRange = -1.0) const;
349 
350  virtual void setOccupancyThres(double prob) = 0;
351  virtual void setProbHit(double prob) = 0;
352  virtual void setProbMiss(double prob) = 0;
353  virtual void setClampingThresMin(double thresProb) = 0;
354  virtual void setClampingThresMax(double thresProb) = 0;
355  virtual double getOccupancyThres() const = 0;
356  virtual float getOccupancyThresLog() const = 0;
357  virtual double getProbHit() const = 0;
358  virtual float getProbHitLog() const = 0;
359  virtual double getProbMiss() const = 0;
360  virtual float getProbMissLog() const = 0;
361  virtual double getClampingThresMin() const = 0;
362  virtual float getClampingThresMinLog() const = 0;
363  virtual double getClampingThresMax() const = 0;
364  virtual float getClampingThresMaxLog() const = 0;
365 
366  protected:
367  /** Builds the list of 3D points in global coordinates for a generic
368  * observation. Used for both, insertObservation() and computeLikelihood().
369  * \param[out] point3d_sensorPt Is a pointer to a "point3D".
370  * \param[out] ptr_scan Is in fact a pointer to "octomap::Pointcloud". Not
371  * declared as such to avoid headers dependencies in user code.
372  * \return false if the observation kind is not applicable.
373  */
374  template <class octomap_point3d, class octomap_pointcloud>
376  const mrpt::obs::CObservation& obs,
377  const mrpt::poses::CPose3D* robotPose, octomap_point3d& sensorPt,
378  octomap_pointcloud& scan) const;
379 
380  struct Impl;
381 
383 
384  private:
385  // See docs in base class
387  const mrpt::obs::CObservation& obs,
388  const mrpt::poses::CPose3D& takenFrom) override;
389 
390 }; // End of class def.
391 } // namespace mrpt::maps
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: COctoMapBase.h:295
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
Definition: COctoMapBase.h:261
virtual void setProbMiss(double prob)=0
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
uint32_t decimation
Speed up the likelihood computation by.
Definition: COctoMapBase.h:250
bool generateGridLines
Generate grid lines for all octree nodes,.
Definition: COctoMapBase.h:263
virtual double getOccupancyThres() const =0
virtual double getClampingThresMin() const =0
TRenderingOptions renderingOptions
Definition: COctoMapBase.h:290
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
spimpl::impl_ptr< T > pimpl
Definition: pimpl.h:15
void setClampingThresMax(double thresProb)
(key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor...
Definition: COctoMapBase.h:137
void setClampingThresMin(double thresProb)
(key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor...
Definition: COctoMapBase.h:130
virtual void setProbHit(double prob)=0
virtual double getClampingThresMax() const =0
TRenderingOptions()=default
generateFreeVoxels=true) (Default=true)
void setOccupancyThres(double prob)
insertion (default: true)
Definition: COctoMapBase.h:111
bool generateOccupiedVoxels
useful to draw the "structure" of the octree, but computationally costly (Default: false) ...
Definition: COctoMapBase.h:269
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
virtual float getProbHitLog() const =0
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
TInsertionOptions & operator=(const TInsertionOptions &o)
Definition: COctoMapBase.h:80
double maxrange
maximum range for how long individual beams are
Definition: COctoMapBase.h:103
TInsertionOptions insertionOptions
The options used when inserting.
Definition: COctoMapBase.h:227
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMapBase.h:45
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
virtual void setClampingThresMin(double thresProb)=0
mrpt::pimpl< Impl > m_impl
Definition: COctoMapBase.h:380
TLikelihoodOptions()
Initilization of default parameters.
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...
TLikelihoodOptions likelihoodOptions
Definition: COctoMapBase.h:254
A wrapper class for pointers whose copy operations from other objects of the same type are ignored...
OCTOMAP_CLASS & getOctomap()
Get a reference to the internal octomap object.
Definition: COctoMapBase.h:61
bool visibleOccupiedVoxels
volumes (Default=true)
Definition: COctoMapBase.h:272
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
COctoMapBase(double resolution)
Constructor, defines the resolution of the octomap (length of each voxel side)
bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose, octomap_point3d &sensorPt, octomap_pointcloud &scan) const
Builds the list of 3D points in global coordinates for a generic observation.
static Ptr Create(Args &&... args)
TInsertionOptions()
Special constructor, not attached to a real COctoMap object: used only in limited situations...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool castRay(const mrpt::math::TPoint3D &origin, const mrpt::math::TPoint3D &direction, mrpt::math::TPoint3D &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Performs raycasting in 3d, similar to computeRay().
bool pruning
inserted (default -1: complete beam)
Definition: COctoMapBase.h:106
virtual float getProbMissLog() const =0
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
const_iterator end() const
Definition: ts_hash_map.h:246
void setProbMiss(double prob)
(key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds)...
Definition: COctoMapBase.h:123
~COctoMapBase() override=default
virtual float getClampingThresMaxLog() const =0
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 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
virtual double getProbMiss() const =0
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
void insertPointCloud(const CPointsMap &ptMap, const float sensor_x, const float sensor_y, const float sensor_z)
Update the octomap with a 2D or 3D scan, given directly as a point cloud and the 3D location of the s...
virtual void setOccupancyThres(double prob)=0
virtual float getClampingThresMinLog() const =0
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void setProbHit(double prob)
(key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) -...
Definition: COctoMapBase.h:117
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
virtual double getProbHit() const =0
virtual float getOccupancyThresLog() const =0
TInsertionOptions(const TInsertionOptions &o)
Definition: COctoMapBase.h:78
With this struct options are provided to the observation insertion process.
Definition: COctoMapBase.h:70
bool generateFreeVoxels
generateOccupiedVoxels=true) (Default=true)
Definition: COctoMapBase.h:277
virtual void setClampingThresMax(double thresProb)=0
mrpt::ignored_copy_ptr< myself_t > m_parent
Definition: COctoMapBase.h:208
virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const =0
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object...



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: fc2d69e97 Fri Jul 10 18:20:57 2020 +0200 at vie jul 10 18:30:13 CEST 2020