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