Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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>
16 #include <octomap/octomap.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  /** See utils::CLoadableOptions */
90  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section);
91  /** See utils::CLoadableOptions */
92  void dumpToTextStream(mrpt::utils::CStream &out) const;
93 
94  double maxrange; //!< maximum range for how long individual beams are inserted (default -1: complete beam)
95  bool pruning; //!< whether the tree is (losslessly) pruned after insertion (default: true)
96 
97  /// (key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0.5)
98  void setOccupancyThres(double prob) { if(m_parent.get()) m_parent->m_octomap.setOccupancyThres(prob); }
99  /// (key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) - sensor model (Default=0.7)
100  void setProbHit(double prob) { if(m_parent.get()) m_parent->m_octomap.setProbHit(prob); }
101  /// (key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds) - sensor model (Default=0.4)
102  void setProbMiss(double prob) { if(m_parent.get()) m_parent->m_octomap.setProbMiss(prob); }
103  /// (key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor model) (Default=0.1192, -2 in log odds)
104  void setClampingThresMin(double thresProb) { if(m_parent.get()) m_parent->m_octomap.setClampingThresMin(thresProb); }
105  /// (key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor model) (Default=0.971, 3.5 in log odds)
106  void setClampingThresMax(double thresProb) { if(m_parent.get()) m_parent->m_octomap.setClampingThresMax(thresProb); }
107 
108  /// @return threshold (probability) for occupancy - sensor model
109  double getOccupancyThres() const { if(m_parent.get()) return m_parent->m_octomap.getOccupancyThres(); else return this->occupancyThres; }
110  /// @return threshold (logodds) for occupancy - sensor model
111  float getOccupancyThresLog() const { return m_parent->m_octomap.getOccupancyThresLog() ; }
112 
113  /// @return probablility for a "hit" in the sensor model (probability)
114  double getProbHit() const { if(m_parent.get()) return m_parent->m_octomap.getProbHit(); else return this->probHit; }
115  /// @return probablility for a "hit" in the sensor model (logodds)
116  float getProbHitLog() const { return m_parent->m_octomap.getProbHitLog(); }
117  /// @return probablility for a "miss" in the sensor model (probability)
118  double getProbMiss() const { if(m_parent.get()) return m_parent->m_octomap.getProbMiss(); else return this->probMiss; }
119  /// @return probablility for a "miss" in the sensor model (logodds)
120  float getProbMissLog() const { return m_parent->m_octomap.getProbMissLog(); }
121 
122  /// @return minimum threshold for occupancy clamping in the sensor model (probability)
123  double getClampingThresMin() const { if(m_parent.get()) return m_parent->m_octomap.getClampingThresMin(); else return this->clampingThresMin; }
124  /// @return minimum threshold for occupancy clamping in the sensor model (logodds)
125  float getClampingThresMinLog() const { return m_parent->m_octomap.getClampingThresMinLog(); }
126  /// @return maximum threshold for occupancy clamping in the sensor model (probability)
127  double getClampingThresMax() const { if(m_parent.get()) return m_parent->m_octomap.getClampingThresMax(); else return this->clampingThresMax; }
128  /// @return maximum threshold for occupancy clamping in the sensor model (logodds)
129  float getClampingThresMaxLog() const { return m_parent->m_octomap.getClampingThresMaxLog(); }
130 
131  private:
133 
134  double occupancyThres; // sets the threshold for occupancy (sensor model) (Default=0.5)
135  double probHit; // sets the probablility for a "hit" (will be converted to logodds) - sensor model (Default=0.7)
136  double probMiss; // sets the probablility for a "miss" (will be converted to logodds) - sensor model (Default=0.4)
137  double clampingThresMin; // sets the minimum threshold for occupancy clamping (sensor model) (Default=0.1192, -2 in log odds)
138  double clampingThresMax; // sets the maximum threshold for occupancy clamping (sensor model) (Default=0.971, 3.5 in log odds)
139  };
140 
141  TInsertionOptions insertionOptions; //!< The options used when inserting observations in the map
142 
143  /** Options used when evaluating "computeObservationLikelihood"
144  * \sa CObservation::computeObservationLikelihood
145  */
147  {
148  /** Initilization of default parameters
149  */
151  virtual ~TLikelihoodOptions() {}
152 
153  /** See utils::CLoadableOptions */
154  void loadFromConfigFile(
155  const mrpt::utils::CConfigFileBase &source,
156  const std::string &section);
157 
158  /** See utils::CLoadableOptions */
159  void dumpToTextStream(mrpt::utils::CStream &out) const;
160 
161  void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream
162  void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream
163 
164  uint32_t decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=1)
165  };
166 
167  TLikelihoodOptions likelihoodOptions;
168 
169  /** Returns true if the map is empty/no observation has been inserted.
170  */
171  virtual bool isEmpty() const;
172 
173 
174 
175  virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const;
176 
177  /** Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels */
179  {
180  bool generateGridLines; //!< Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
181 
182  bool generateOccupiedVoxels; //!< Generate voxels for the occupied volumes (Default=true)
183  bool visibleOccupiedVoxels; //!< Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
184 
185  bool generateFreeVoxels; //!< Generate voxels for the freespace (Default=true)
186  bool visibleFreeVoxels; //!< Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
187 
189  generateGridLines (false),
190  generateOccupiedVoxels (true),
191  visibleOccupiedVoxels (true),
192  generateFreeVoxels (true),
193  visibleFreeVoxels (true)
194  { }
195 
196  void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream
197  void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream
198  };
199 
200  TRenderingOptions renderingOptions;
201 
202  /** Returns a 3D object representing the map.
203  * \sa renderingOptions
204  */
205  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
206  {
207  mrpt::opengl::COctoMapVoxelsPtr gl_obj = mrpt::opengl::COctoMapVoxels::Create();
208  this->getAsOctoMapVoxels(*gl_obj);
209  outObj->insert(gl_obj);
210  }
211 
212  /** Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object.
213  * \sa renderingOptions
214  */
215  virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const = 0;
216 
217  /** Check whether the given point lies within the volume covered by the octomap (that is, whether it is "mapped") */
218  bool isPointWithinOctoMap(const float x,const float y,const float z) const
219  {
220  octomap::OcTreeKey key;
221  return m_octomap.coordToKeyChecked(octomap::point3d(x,y,z), key);
222  }
223 
224  /** Get the occupancy probability [0,1] of a point
225  * \return false if the point is not mapped, in which case the returned "prob" is undefined. */
226  bool getPointOccupancy(const float x,const float y,const float z, double &prob_occupancy) const;
227 
228  /** 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 */
229  void updateVoxel(const double x, const double y, const double z, bool occupied)
230  {
231  m_octomap.updateNode(x,y,z, occupied);
232  }
233 
234  /** 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.
235  * Insertion parameters can be found in \a insertionOptions.
236  * \sa The generic observation insertion method CMetricMap::insertObservation()
237  */
238  void insertPointCloud(const CPointsMap &ptMap, const float sensor_x,const float sensor_y,const float sensor_z);
239 
240  /** Just like insertPointCloud but with a single ray. */
241  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)
242  {
243  m_octomap.insertRay( octomap::point3d(sensor_x,sensor_y,sensor_z), octomap::point3d(end_x,end_y,end_z), insertionOptions.maxrange,insertionOptions.pruning);
244  }
245 
246  /** Performs raycasting in 3d, similar to computeRay().
247  *
248  * A ray is cast from origin with a given direction, the first occupied
249  * cell is returned (as center coordinate). If the starting coordinate is already
250  * occupied in the tree, this coordinate will be returned as a hit.
251  *
252  * @param origin starting coordinate of ray
253  * @param direction A vector pointing in the direction of the raycast. Does not need to be normalized.
254  * @param end returns the center of the cell that was hit by the ray, if successful
255  * @param ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit.
256  * @param maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default)
257  * @return whether or not an occupied cell was hit
258  */
259  bool castRay(
260  const mrpt::math::TPoint3D & origin,
261  const mrpt::math::TPoint3D & direction,
263  bool ignoreUnknownCells=false,
264  double maxRange=-1.0) const;
265 
266  /** @name Direct access to octomap library methods
267  @{ */
268 
269  double getResolution() const { return m_octomap.getResolution(); }
270  unsigned int getTreeDepth () const { return m_octomap.getTreeDepth(); }
271  /// \return The number of nodes in the tree
272  size_t size() const { return m_octomap.size(); }
273  /// \return Memory usage of the complete octree in bytes (may vary between architectures)
274  size_t memoryUsage() const { return m_octomap.memoryUsage(); }
275  /// \return Memory usage of the a single octree node
276  size_t memoryUsageNode() const { return m_octomap.memoryUsageNode(); }
277  /// \return Memory usage of a full grid of the same size as the OcTree in bytes (for comparison)
278  size_t memoryFullGrid() const { return m_octomap.memoryFullGrid(); }
279  double volume() const { return m_octomap.volume(); }
280  /// Size of OcTree (all known space) in meters for x, y and z dimension
281  void getMetricSize(double& x, double& y, double& z) { return m_octomap.getMetricSize(x,y,z); }
282  /// Size of OcTree (all known space) in meters for x, y and z dimension
283  void getMetricSize(double& x, double& y, double& z) const { return m_octomap.getMetricSize(x,y,z); }
284  /// minimum value of the bounding box of all known space in x, y, z
285  void getMetricMin(double& x, double& y, double& z) { return m_octomap.getMetricMin(x,y,z); }
286  /// minimum value of the bounding box of all known space in x, y, z
287  void getMetricMin(double& x, double& y, double& z) const { return m_octomap.getMetricMin(x,y,z); }
288  /// maximum value of the bounding box of all known space in x, y, z
289  void getMetricMax(double& x, double& y, double& z) { return m_octomap.getMetricMax(x,y,z); }
290  /// maximum value of the bounding box of all known space in x, y, z
291  void getMetricMax(double& x, double& y, double& z) const { return m_octomap.getMetricMax(x,y,z); }
292 
293  /// Traverses the tree to calculate the total number of nodes
294  size_t calcNumNodes() const { return m_octomap.calcNumNodes(); }
295 
296  /// Traverses the tree to calculate the total number of leaf nodes
297  size_t getNumLeafNodes() const { return m_octomap.getNumLeafNodes(); }
298 
299  /** @} */
300 
301 
302  protected:
303  virtual void internal_clear() { m_octomap.clear(); }
304 
305  /** Builds the list of 3D points in global coordinates for a generic observation. Used for both, insertObservation() and computeLikelihood().
306  * \param[out] point3d_sensorPt Is a pointer to a "point3D".
307  * \param[out] ptr_scan Is in fact a pointer to "octomap::Pointcloud". Not declared as such to avoid headers dependencies in user code.
308  * \return false if the observation kind is not applicable.
309  */
310  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;
311 
312  OCTREE m_octomap; //!< The actual octo-map object.
313 
314  private:
315  // See docs in base class
317 
318  }; // End of class def.
319  } // End of namespace
320 } // End of namespace
321 
322 #include "COctoMapBase_impl.h"
323 
324 #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.
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
virtual void internal_clear()
Internal method called by clear()
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
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:
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 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
void getMetricSize(double &x, double &y, double &z) const
Size of OcTree (all known space) in meters for x, y and z dimension.
virtual bool isEmpty() const
Returns true if the map is empty/no observation has been inserted.
unsigned int getTreeDepth() const
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.
void dumpToTextStream(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
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.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
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...
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)
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.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
void getMetricMin(double &x, double &y, double &z) const
minimum value of the bounding box of all known space in x, y, z
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations...
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().
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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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)
Declares a virtual base class for all metric maps storage classes.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const
Returns a 3D object representing the map.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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...
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
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.
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".
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...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
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
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.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:22:34 UTC 2016