MRPT  2.0.1
COctoMap.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 
12 #include <mrpt/maps/COctoMapBase.h>
13 
16 #include <mrpt/maps/CMetricMap.h>
17 #include <mrpt/obs/obs_frwds.h>
18 
19 namespace octomap
20 {
21 class OcTree;
22 }
23 namespace octomap
24 {
25 class OcTreeNode;
26 }
27 
28 namespace mrpt
29 {
30 namespace maps
31 {
32 /** A three-dimensional probabilistic occupancy grid, implemented as an
33  * octo-tree with the "octomap" C++ library.
34  * This version only stores occupancy information at each octree node. See the
35  * base class mrpt::maps::COctoMapBase.
36  *
37  * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
38  * \ingroup mrpt_maps_grp
39  */
40 class COctoMap : public COctoMapBase<octomap::OcTree, octomap::OcTreeNode>
41 {
42  // This must be added to any CSerializable derived class:
44 
45  public:
46  COctoMap(const double resolution = 0.10); //!< Default constructor
47  ~COctoMap() override; //!< Destructor
48 
49  void getAsOctoMapVoxels(
50  mrpt::opengl::COctoMapVoxels& gl_obj) const override;
51 
53  double resolution{
54  0.10}; //!< The finest resolution of the octomap (default: 0.10
55  //! meters)
56  mrpt::maps::COctoMap::TInsertionOptions
57  insertionOpts; //!< Observations insertion options
58  mrpt::maps::COctoMap::TLikelihoodOptions
59  likelihoodOpts; //!< Probabilistic observation likelihood options
61 
62  /** Returns true if the map is empty/no observation has been inserted */
63  bool isEmpty() const override { return size() == 1; }
64  /** @name Direct access to octomap library methods
65  @{ */
66 
67  /** Just like insertPointCloud but with a single ray. */
68  void insertRay(
69  const float end_x, const float end_y, const float end_z,
70  const float sensor_x, const float sensor_y, const float sensor_z);
71  /** Manually updates the occupancy of the voxel at (x,y,z) as being occupied
72  * (true) or free (false), using the log-odds parameters in \a
73  * insertionOptions */
74  void updateVoxel(
75  const double x, const double y, const double z, bool occupied);
76  /** Check whether the given point lies within the volume covered by the
77  * octomap (that is, whether it is "mapped") */
79  const float x, const float y, const float z) const;
80  double getResolution() const;
81  unsigned int getTreeDepth() const;
82  /// \return The number of nodes in the tree
83  size_t size() const;
84  /// \return Memory usage of the complete octree in bytes (may vary between
85  /// architectures)
86  size_t memoryUsage() const;
87  /// \return Memory usage of the a single octree node
88  size_t memoryUsageNode() const;
89  /// \return Memory usage of a full grid of the same size as the OcTree in
90  /// bytes (for comparison)
91  size_t memoryFullGrid() const;
92  double volume();
93  /// Size of OcTree (all known space) in meters for x, y and z dimension
94  void getMetricSize(double& x, double& y, double& z);
95  /// Size of OcTree (all known space) in meters for x, y and z dimension
96  void getMetricSize(double& x, double& y, double& z) const;
97  /// minimum value of the bounding box of all known space in x, y, z
98  void getMetricMin(double& x, double& y, double& z);
99  /// minimum value of the bounding box of all known space in x, y, z
100  void getMetricMin(double& x, double& y, double& z) const;
101  /// maximum value of the bounding box of all known space in x, y, z
102  void getMetricMax(double& x, double& y, double& z);
103  /// maximum value of the bounding box of all known space in x, y, z
104  void getMetricMax(double& x, double& y, double& z) const;
105  /// Traverses the tree to calculate the total number of nodes
106  size_t calcNumNodes() const;
107  /// Traverses the tree to calculate the total number of leaf nodes
108  size_t getNumLeafNodes() const;
109 
110  void setOccupancyThres(double prob) override;
111  void setProbHit(double prob) override;
112  void setProbMiss(double prob) override;
113  void setClampingThresMin(double thresProb) override;
114  void setClampingThresMax(double thresProb) override;
115  double getOccupancyThres() const override;
116  float getOccupancyThresLog() const override;
117  double getProbHit() const override;
118  float getProbHitLog() const override;
119  double getProbMiss() const override;
120  float getProbMissLog() const override;
121  double getClampingThresMin() const override;
122  float getClampingThresMinLog() const override;
123  double getClampingThresMax() const override;
124  float getClampingThresMaxLog() const override;
125  /** @} */
126 
127  protected:
128  void internal_clear() override;
130  const mrpt::obs::CObservation& obs,
131  const mrpt::poses::CPose3D* robotPose) override;
132 }; // End of class def.
133 } // namespace maps
134 } // namespace mrpt
void setClampingThresMax(double thresProb) override
Definition: COctoMap.cpp:371
double getOccupancyThres() const override
Definition: COctoMap.cpp:375
void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
Definition: COctoMap.cpp:329
float getClampingThresMaxLog() const override
Definition: COctoMap.cpp:405
size_t memoryUsageNode() const
Definition: COctoMap.cpp:320
double getClampingThresMin() const override
Definition: COctoMap.cpp:393
double getProbHit() const override
Definition: COctoMap.cpp:383
void setProbHit(double prob) override
Definition: COctoMap.cpp:365
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
size_t memoryFullGrid() const
Definition: COctoMap.cpp:324
float getClampingThresMinLog() const override
Definition: COctoMap.cpp:397
float getOccupancyThresLog() const override
Definition: COctoMap.cpp:379
void setClampingThresMin(double thresProb) override
Definition: COctoMap.cpp:367
void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
Definition: COctoMap.cpp:345
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose) override
Internal method called by insertObservation()
Definition: COctoMap.cpp:137
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).
double getClampingThresMax() const override
Definition: COctoMap.cpp:401
float getProbMissLog() const override
Definition: COctoMap.cpp:389
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const override
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object...
Definition: COctoMap.cpp:153
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)...
Definition: COctoMap.cpp:298
void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
Definition: COctoMap.cpp:337
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.
Definition: COctoMap.cpp:289
void internal_clear() override
Internal method called by clear()
Definition: COctoMap.cpp:409
void setOccupancyThres(double prob) override
Definition: COctoMap.cpp:361
mrpt::maps::COctoMap::TInsertionOptions insertionOpts
meters)
Definition: COctoMap.h:57
mrpt::maps::COctoMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: COctoMap.h:59
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: COctoMap.h:63
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
Definition: COctoMap.cpp:357
double getResolution() const
Definition: COctoMap.cpp:310
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMap.h:40
double getProbMiss() const override
Definition: COctoMap.cpp:388
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...
Definition: COctoMap.cpp:303
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
size_t size() const
Definition: COctoMap.cpp:318
void setProbMiss(double prob) override
Definition: COctoMap.cpp:366
size_t memoryUsage() const
Definition: COctoMap.cpp:319
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
Definition: COctoMap.cpp:353
unsigned int getTreeDepth() const
Definition: COctoMap.cpp:314
~COctoMap() override
Destructor.
#define MAP_DEFINITION_END(_CLASS_NAME_)
float getProbHitLog() const override
Definition: COctoMap.cpp:384
COctoMap(const double resolution=0.10)
Default constructor.
Definition: COctoMap.cpp:80



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020