Main MRPT website > C++ reference for MRPT 1.4.0
maps/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-2016, 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>
19 #include <mrpt/obs/obs_frwds.h>
20 
21 #include <mrpt/maps/link_pragmas.h>
22 
23 namespace mrpt
24 {
25  namespace maps
26  {
27  /** A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ library.
28  * This base class represents a 3D map where each voxel may contain an "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 of the map calling getAs3DObject() or getAsOctoMapVoxels()
31  *
32  * To use octomap's iterators to go through the voxels, use COctoMap::getOctomap()
33  *
34  * The octomap library was presented in:
35  * - K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard,
36  * <i>"OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation for Robotic Systems"</i>
37  * in Proc. of the ICRA 2010 Workshop on Best Practice in 3D Perception and Modeling for Mobile Manipulation, 2010. Software available at http://octomap.sf.net/.
38  *
39  * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
40  * \ingroup mrpt_maps_grp
41  */
42  template <class OCTREE,class OCTREE_NODE>
44  {
45  public:
46  typedef COctoMapBase<OCTREE,OCTREE_NODE> myself_t; //!< The type of this MRPT class
47  typedef OCTREE octree_t; //!< The type of the octree class in the "octomap" library.
48  typedef OCTREE_NODE octree_node_t; //!< The type of nodes in the octree, in the "octomap" library.
49 
50 
51  /** Constructor, defines the resolution of the octomap (length of each voxel side) */
52  COctoMapBase(const double resolution=0.10) : insertionOptions(*this), m_octomap(resolution) { }
53  virtual ~COctoMapBase() { }
54 
55  /** Get a reference to the internal octomap object. Example:
56  * \code
57  * mrpt::maps::COctoMap map;
58  * ...
59  * octomap::OcTree &om = map.getOctomap();
60  *
61  *
62  * \endcode
63  */
64  inline OCTREE & getOctomap() { return m_octomap; }
65 
66  /** With this struct options are provided to the observation insertion process.
67  * \sa CObservation::insertObservationInto()
68  */
70  {
71  /** Initilization of default parameters */
72  TInsertionOptions( myself_t &parent );
73 
74  TInsertionOptions(); //!< Especial constructor, not attached to a real COctoMap object: used only in limited situations, since get*() methods don't work, etc.
76  {
77  // Copy all but the m_parent pointer!
78  maxrange = o.maxrange;
79  pruning = o.pruning;
80  const bool o_has_parent = o.m_parent.get()!=NULL;
81  setOccupancyThres( o_has_parent ? o.getOccupancyThres() : o.occupancyThres );
82  setProbHit( o_has_parent ? o.getProbHit() : o.probHit );
83  setProbMiss( o_has_parent ? o.getProbMiss() : o.probMiss );
86  return *this;
87  }
88 
89  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
90  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
91 
92  double maxrange; //!< maximum range for how long individual beams are inserted (default -1: complete beam)
93  bool pruning; //!< whether the tree is (losslessly) pruned after insertion (default: true)
94 
95  /// (key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0.5)
96  void setOccupancyThres(double prob) { if(m_parent.get()) m_parent->m_octomap.setOccupancyThres(prob); }
97  /// (key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) - sensor model (Default=0.7)
98  void setProbHit(double prob) { if(m_parent.get()) m_parent->m_octomap.setProbHit(prob); }
99  /// (key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds) - sensor model (Default=0.4)
100  void setProbMiss(double prob) { if(m_parent.get()) m_parent->m_octomap.setProbMiss(prob); }
101  /// (key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor model) (Default=0.1192, -2 in log odds)
102  void setClampingThresMin(double thresProb) { if(m_parent.get()) m_parent->m_octomap.setClampingThresMin(thresProb); }
103  /// (key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor model) (Default=0.971, 3.5 in log odds)
104  void setClampingThresMax(double thresProb) { if(m_parent.get()) m_parent->m_octomap.setClampingThresMax(thresProb); }
105 
106  /// @return threshold (probability) for occupancy - sensor model
107  double getOccupancyThres() const { if(m_parent.get()) return m_parent->m_octomap.getOccupancyThres(); else return this->occupancyThres; }
108  /// @return threshold (logodds) for occupancy - sensor model
109  float getOccupancyThresLog() const { return m_parent->m_octomap.getOccupancyThresLog() ; }
110 
111  /// @return probablility for a "hit" in the sensor model (probability)
112  double getProbHit() const { if(m_parent.get()) return m_parent->m_octomap.getProbHit(); else return this->probHit; }
113  /// @return probablility for a "hit" in the sensor model (logodds)
114  float getProbHitLog() const { return m_parent->m_octomap.getProbHitLog(); }
115  /// @return probablility for a "miss" in the sensor model (probability)
116  double getProbMiss() const { if(m_parent.get()) return m_parent->m_octomap.getProbMiss(); else return this->probMiss; }
117  /// @return probablility for a "miss" in the sensor model (logodds)
118  float getProbMissLog() const { return m_parent->m_octomap.getProbMissLog(); }
119 
120  /// @return minimum threshold for occupancy clamping in the sensor model (probability)
121  double getClampingThresMin() const { if(m_parent.get()) return m_parent->m_octomap.getClampingThresMin(); else return this->clampingThresMin; }
122  /// @return minimum threshold for occupancy clamping in the sensor model (logodds)
123  float getClampingThresMinLog() const { return m_parent->m_octomap.getClampingThresMinLog(); }
124  /// @return maximum threshold for occupancy clamping in the sensor model (probability)
125  double getClampingThresMax() const { if(m_parent.get()) return m_parent->m_octomap.getClampingThresMax(); else return this->clampingThresMax; }
126  /// @return maximum threshold for occupancy clamping in the sensor model (logodds)
127  float getClampingThresMaxLog() const { return m_parent->m_octomap.getClampingThresMaxLog(); }
128 
129  private:
131 
132  double occupancyThres; // sets the threshold for occupancy (sensor model) (Default=0.5)
133  double probHit; // sets the probablility for a "hit" (will be converted to logodds) - sensor model (Default=0.7)
134  double probMiss; // sets the probablility for a "miss" (will be converted to logodds) - sensor model (Default=0.4)
135  double clampingThresMin; // sets the minimum threshold for occupancy clamping (sensor model) (Default=0.1192, -2 in log odds)
136  double clampingThresMax; // sets the maximum threshold for occupancy clamping (sensor model) (Default=0.971, 3.5 in log odds)
137  };
138 
139  TInsertionOptions insertionOptions; //!< The options used when inserting observations in the map
140 
141  /** Options used when evaluating "computeObservationLikelihood"
142  * \sa CObservation::computeObservationLikelihood
143  */
145  {
146  /** Initilization of default parameters
147  */
149  virtual ~TLikelihoodOptions() {}
150 
151  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
152  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
153 
154  void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream
155  void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream
156 
157  uint32_t decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=1)
158  };
159 
160  TLikelihoodOptions likelihoodOptions;
161 
162  /** Returns true if the map is empty/no observation has been inserted */
163  virtual bool isEmpty() const MRPT_OVERRIDE;
164 
165  virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
166 
167  /** Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels */
169  {
170  bool generateGridLines; //!< Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
171 
172  bool generateOccupiedVoxels; //!< Generate voxels for the occupied volumes (Default=true)
173  bool visibleOccupiedVoxels; //!< Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
174 
175  bool generateFreeVoxels; //!< Generate voxels for the freespace (Default=true)
176  bool visibleFreeVoxels; //!< Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
177 
179  generateGridLines (false),
180  generateOccupiedVoxels (true),
181  visibleOccupiedVoxels (true),
182  generateFreeVoxels (true),
183  visibleFreeVoxels (true)
184  { }
185 
186  void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream
187  void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream
188  };
189 
190  TRenderingOptions renderingOptions;
191 
192  /** Returns a 3D object representing the map.
193  * \sa renderingOptions
194  */
195  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const MRPT_OVERRIDE
196  {
197  mrpt::opengl::COctoMapVoxelsPtr gl_obj = mrpt::opengl::COctoMapVoxels::Create();
198  this->getAsOctoMapVoxels(*gl_obj);
199  outObj->insert(gl_obj);
200  }
201 
202  /** Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object.
203  * \sa renderingOptions
204  */
205  virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const = 0;
206 
207  /** Check whether the given point lies within the volume covered by the octomap (that is, whether it is "mapped") */
208  bool isPointWithinOctoMap(const float x,const float y,const float z) const
209  {
210  octomap::OcTreeKey key;
211  return m_octomap.coordToKeyChecked(octomap::point3d(x,y,z), key);
212  }
213 
214  /** Get the occupancy probability [0,1] of a point
215  * \return false if the point is not mapped, in which case the returned "prob" is undefined. */
216  bool getPointOccupancy(const float x,const float y,const float z, double &prob_occupancy) const;
217 
218  /** Manually updates the occupancy of the voxel at (x,y,z) as being occupied (true) or free (false), using the log-odds parameters in \a insertionOptions */
219  void updateVoxel(const double x, const double y, const double z, bool occupied)
220  {
221  m_octomap.updateNode(x,y,z, occupied);
222  }
223 
224  /** Update the octomap with a 2D or 3D scan, given directly as a point cloud and the 3D location of the sensor (the origin of the rays) in this map's frame of reference.
225  * Insertion parameters can be found in \a insertionOptions.
226  * \sa The generic observation insertion method CMetricMap::insertObservation()
227  */
228  void insertPointCloud(const CPointsMap &ptMap, const float sensor_x,const float sensor_y,const float sensor_z);
229 
230  /** Just like insertPointCloud but with a single ray. */
231  void insertRay(const float end_x,const float end_y,const float end_z,const float sensor_x,const float sensor_y,const float sensor_z)
232  {
233  m_octomap.insertRay( octomap::point3d(sensor_x,sensor_y,sensor_z), octomap::point3d(end_x,end_y,end_z), insertionOptions.maxrange,insertionOptions.pruning);
234  }
235 
236  /** Performs raycasting in 3d, similar to computeRay().
237  *
238  * A ray is cast from origin with a given direction, the first occupied
239  * cell is returned (as center coordinate). If the starting coordinate is already
240  * occupied in the tree, this coordinate will be returned as a hit.
241  *
242  * @param origin starting coordinate of ray
243  * @param direction A vector pointing in the direction of the raycast. Does not need to be normalized.
244  * @param end returns the center of the cell that was hit by the ray, if successful
245  * @param ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit.
246  * @param maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default)
247  * @return whether or not an occupied cell was hit
248  */
249  bool castRay(
250  const mrpt::math::TPoint3D & origin,
251  const mrpt::math::TPoint3D & direction,
253  bool ignoreUnknownCells=false,
254  double maxRange=-1.0) const;
255 
256  /** @name Direct access to octomap library methods
257  @{ */
258 
259  double getResolution() const { return m_octomap.getResolution(); }
260  unsigned int getTreeDepth () const { return m_octomap.getTreeDepth(); }
261  /// \return The number of nodes in the tree
262  size_t size() const { return m_octomap.size(); }
263  /// \return Memory usage of the complete octree in bytes (may vary between architectures)
264  size_t memoryUsage() const { return m_octomap.memoryUsage(); }
265  /// \return Memory usage of the a single octree node
266  size_t memoryUsageNode() const { return m_octomap.memoryUsageNode(); }
267  /// \return Memory usage of a full grid of the same size as the OcTree in bytes (for comparison)
268  size_t memoryFullGrid() const { return m_octomap.memoryFullGrid(); }
269  double volume() const { return m_octomap.volume(); }
270  /// Size of OcTree (all known space) in meters for x, y and z dimension
271  void getMetricSize(double& x, double& y, double& z) { return m_octomap.getMetricSize(x,y,z); }
272  /// Size of OcTree (all known space) in meters for x, y and z dimension
273  void getMetricSize(double& x, double& y, double& z) const { return m_octomap.getMetricSize(x,y,z); }
274  /// minimum value of the bounding box of all known space in x, y, z
275  void getMetricMin(double& x, double& y, double& z) { return m_octomap.getMetricMin(x,y,z); }
276  /// minimum value of the bounding box of all known space in x, y, z
277  void getMetricMin(double& x, double& y, double& z) const { return m_octomap.getMetricMin(x,y,z); }
278  /// maximum value of the bounding box of all known space in x, y, z
279  void getMetricMax(double& x, double& y, double& z) { return m_octomap.getMetricMax(x,y,z); }
280  /// maximum value of the bounding box of all known space in x, y, z
281  void getMetricMax(double& x, double& y, double& z) const { return m_octomap.getMetricMax(x,y,z); }
282 
283  /// Traverses the tree to calculate the total number of nodes
284  size_t calcNumNodes() const { return m_octomap.calcNumNodes(); }
285 
286  /// Traverses the tree to calculate the total number of leaf nodes
287  size_t getNumLeafNodes() const { return m_octomap.getNumLeafNodes(); }
288 
289  /** @} */
290 
291 
292  protected:
293  virtual void internal_clear() MRPT_OVERRIDE { m_octomap.clear(); }
294 
295  /** Builds the list of 3D points in global coordinates for a generic observation. Used for both, insertObservation() and computeLikelihood().
296  * \param[out] point3d_sensorPt Is a pointer to a "point3D".
297  * \param[out] ptr_scan Is in fact a pointer to "octomap::Pointcloud". Not declared as such to avoid headers dependencies in user code.
298  * \return false if the observation kind is not applicable.
299  */
300  bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs,const mrpt::poses::CPose3D *robotPose, octomap::point3d &point3d_sensorPt, octomap::Pointcloud &ptr_scan) const;
301 
302  OCTREE m_octomap; //!< The actual octo-map object.
303 
304  private:
305  // See docs in base class
307 
308  }; // End of class def.
309  } // End of namespace
310 } // End of namespace
311 
312 #include "COctoMapBase_impl.h"
313 
314 #endif
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
void setOccupancyThres(double prob)
(key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0...
void updateVoxel(const double x, const double y, const double z, bool occupied)
Manually updates the occupancy of the voxel at (x,y,z) as being occupied (true) or free (false)...
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
OCTREE m_octomap
The actual octo-map object.
OCTREE & getOctomap()
Get a reference to the internal octomap object.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose, octomap::point3d &point3d_sensorPt, octomap::Pointcloud &ptr_scan) const
Builds the list of 3D points in global coordinates for a generic observation.
virtual void writeToStream(mrpt::utils::CStream &out, int *getVersion) const =0
Introduces a pure virtual method responsible for writing to a CStream.
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true) ...
TLikelihoodOptions likelihoodOptions
virtual void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
STL namespace.
void insertRay(const float end_x, const float end_y, const float end_z, const float sensor_x, const float sensor_y, const float sensor_z)
Just like insertPointCloud but with a single ray.
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
OCTREE_NODE octree_node_t
The type of nodes in the octree, in the "octomap" library.
This class allows loading and storing values and vectors of different types from a configuration text...
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
void getMetricSize(double &x, double &y, double &z) const
Size of OcTree (all known space) in meters for x, y and z dimension.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
unsigned int getTreeDepth() const
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations...
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
Definition: Pointcloud.h:63
void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
TInsertionOptions & operator=(const TInsertionOptions &o)
bool pruning
whether the tree is (losslessly) pruned after insertion (default: true)
void setClampingThresMax(double thresProb)
(key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor...
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().
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
size_t memoryUsageNode() const
bool isPointWithinOctoMap(const float x, const float y, const float z) const
Check whether the given point lies within the volume covered by the octomap (that is...
void setProbHit(double prob)
(key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) -...
COctoMapBase< OCTREE, OCTREE_NODE > myself_t
The type of this MRPT class.
void setProbMiss(double prob)
(key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds)...
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
double maxrange
maximum range for how long individual beams are inserted (default -1: complete beam) ...
Declares a class that represents any robot&#39;s observation.
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...
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:68
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=1) ...
void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
With this struct options are provided to the observation insertion process.
size_t memoryFullGrid() const
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
OCTREE octree_t
The type of the octree class in the "octomap" library.
static COctoMapVoxelsPtr Create()
Lightweight 3D point.
mrpt::utils::ignored_copy_ptr< myself_t > m_parent
COctoMapBase(const double resolution=0.10)
Constructor, defines the resolution of the octomap (length of each voxel side)
Options used when evaluating "computeObservationLikelihood".
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
void setClampingThresMin(double thresProb)
(key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor...
A wrapper class for pointers whose copy operations from other objects of the same type are ignored...
virtual void readFromStream(mrpt::utils::CStream &in, int version)=0
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
void getMetricMin(double &x, double &y, double &z) const
minimum value of the bounding box of all known space in x, y, z
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void getMetricMax(double &x, double &y, double &z) const
maximum value of the bounding box of all known space in x, y, z
virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const =0
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object...
TRenderingOptions renderingOptions
This class represents a three-dimensional vector.
Definition: Vector3.h:65
void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN: at Mon Aug 15 11:50:21 UTC 2016