MRPT  2.0.4
CColouredOctoMap.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 #include <mrpt/obs/obs_frwds.h>
14 
15 namespace octomap
16 {
17 class ColorOcTree;
18 }
19 namespace octomap
20 {
21 class ColorOcTreeNode;
22 }
23 
24 namespace mrpt
25 {
26 namespace maps
27 {
28 /** A three-dimensional probabilistic occupancy grid, implemented as an
29  * octo-tree with the "octomap" C++ library.
30  * This version stores both, occupancy information and RGB colour data at
31  * each octree node. See the base class mrpt::maps::COctoMapBase.
32  *
33  * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
34  * \ingroup mrpt_maps_grp
35  */
37  : public COctoMapBase<octomap::ColorOcTree, octomap::ColorOcTreeNode>
38 {
39  // This must be added to any CSerializable derived class:
41 
42  public:
43  CColouredOctoMap(const double resolution = 0.10); //!< Default constructor
44  ~CColouredOctoMap() override; //!< Destructor
45 
46  /** This allows the user to select the desired method to update voxels
47  colour.
48  SET = Set the colour of the voxel at (x,y,z) directly
49  AVERAGE = Set the colour of the voxel at (x,y,z) as the mean of
50  its previous colour and the new observed one.
51  INTEGRATE = Calculate the new colour of the voxel at (x,y,z) using
52  this formula: prev_color*node_prob + new_color*(0.99-node_prob)
53  If there isn't any previous color, any method is equivalent to
54  SET.
55  INTEGRATE is the default option*/
57  {
58  INTEGRATE = 0,
59  SET,
60  AVERAGE
61  };
62 
63  /** Get the RGB colour of a point
64  * \return false if the point is not mapped, in which case the
65  * returned colour is undefined. */
66  bool getPointColour(
67  const float x, const float y, const float z, uint8_t& r, uint8_t& g,
68  uint8_t& b) const;
69 
70  /** Manually update the colour of the voxel at (x,y,z) */
71  void updateVoxelColour(
72  const double x, const double y, const double z, const uint8_t r,
73  const uint8_t g, const uint8_t b);
74 
75  /// Set the method used to update voxels colour
77  {
78  m_colour_method = new_method;
79  }
80 
81  /// Get the method used to update voxels colour
83  void getAsOctoMapVoxels(
84  mrpt::opengl::COctoMapVoxels& gl_obj) const override;
85 
87  double resolution{
88  0.10}; //!< The finest resolution of the octomap (default: 0.10
89  //! meters)
90  mrpt::maps::CColouredOctoMap::TInsertionOptions
91  insertionOpts; //!< Observations insertion options
92  mrpt::maps::CColouredOctoMap::TLikelihoodOptions
93  likelihoodOpts; //!< Probabilistic observation likelihood options
95 
96  /** Returns true if the map is empty/no observation has been inserted */
97  bool isEmpty() const override { return size() == 1; }
98  /** @name Direct access to octomap library methods
99  @{ */
100 
101  /** Just like insertPointCloud but with a single ray. */
102  void insertRay(
103  const float end_x, const float end_y, const float end_z,
104  const float sensor_x, const float sensor_y, const float sensor_z);
105  /** Manually updates the occupancy of the voxel at (x,y,z) as being occupied
106  * (true) or free (false), using the log-odds parameters in \a
107  * insertionOptions */
108  void updateVoxel(
109  const double x, const double y, const double z, bool occupied);
110  /** Check whether the given point lies within the volume covered by the
111  * octomap (that is, whether it is "mapped") */
113  const float x, const float y, const float z) const;
114  double getResolution() const;
115  unsigned int getTreeDepth() const;
116  /// \return The number of nodes in the tree
117  size_t size() const;
118  /// \return Memory usage of the complete octree in bytes (may vary between
119  /// architectures)
120  size_t memoryUsage() const;
121  /// \return Memory usage of the a single octree node
122  size_t memoryUsageNode() const;
123  /// \return Memory usage of a full grid of the same size as the OcTree in
124  /// bytes (for comparison)
125  size_t memoryFullGrid() const;
126  double volume();
127  /// Size of OcTree (all known space) in meters for x, y and z dimension
128  void getMetricSize(double& x, double& y, double& z);
129  /// Size of OcTree (all known space) in meters for x, y and z dimension
130  void getMetricSize(double& x, double& y, double& z) const;
131  /// minimum value of the bounding box of all known space in x, y, z
132  void getMetricMin(double& x, double& y, double& z);
133  /// minimum value of the bounding box of all known space in x, y, z
134  void getMetricMin(double& x, double& y, double& z) const;
135  /// maximum value of the bounding box of all known space in x, y, z
136  void getMetricMax(double& x, double& y, double& z);
137  /// maximum value of the bounding box of all known space in x, y, z
138  void getMetricMax(double& x, double& y, double& z) const;
139  /// Traverses the tree to calculate the total number of nodes
140  size_t calcNumNodes() const;
141  /// Traverses the tree to calculate the total number of leaf nodes
142  size_t getNumLeafNodes() const;
143 
144  void setOccupancyThres(double prob) override;
145  void setProbHit(double prob) override;
146  void setProbMiss(double prob) override;
147  void setClampingThresMin(double thresProb) override;
148  void setClampingThresMax(double thresProb) override;
149  double getOccupancyThres() const override;
150  float getOccupancyThresLog() const override;
151  double getProbHit() const override;
152  float getProbHitLog() const override;
153  double getProbMiss() const override;
154  float getProbMissLog() const override;
155  double getClampingThresMin() const override;
156  float getClampingThresMinLog() const override;
157  double getClampingThresMax() const override;
158  float getClampingThresMaxLog() const override;
159  /** @} */
160 
161  protected:
162  void internal_clear() override;
163 
165  const mrpt::obs::CObservation& obs,
166  const mrpt::poses::CPose3D* robotPose) override;
167 
169  INTEGRATE}; //! Method used to updated voxels colour.
170 
171 }; // End of class def.
172 } // namespace maps
173 } // namespace mrpt
mrpt::maps::CColouredOctoMap::isEmpty
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Definition: CColouredOctoMap.h:97
mrpt::maps::CColouredOctoMap::setProbMiss
void setProbMiss(double prob) override
Definition: CColouredOctoMap.cpp:491
mrpt::maps::CColouredOctoMap::getClampingThresMaxLog
float getClampingThresMaxLog() const override
Definition: CColouredOctoMap.cpp:539
mrpt::maps::CColouredOctoMap::getMetricMax
void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
Definition: CColouredOctoMap.cpp:467
mrpt::maps::CColouredOctoMap::updateVoxel
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: CColouredOctoMap.cpp:417
mrpt::maps::CColouredOctoMap::volume
double volume()
Definition: CColouredOctoMap.cpp:450
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
mrpt::maps::CColouredOctoMap::memoryUsage
size_t memoryUsage() const
Definition: CColouredOctoMap.cpp:438
mrpt::maps::CColouredOctoMap::getMetricSize
void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
Definition: CColouredOctoMap.cpp:451
mrpt::maps::CColouredOctoMap::memoryUsageNode
size_t memoryUsageNode() const
Definition: CColouredOctoMap.cpp:442
mrpt::maps::CColouredOctoMap::internal_insertObservation
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose) override
Internal method called by insertObservation()
Definition: CColouredOctoMap.cpp:150
mrpt::maps::CColouredOctoMap::getOccupancyThres
double getOccupancyThres() const override
Definition: CColouredOctoMap.cpp:503
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:16
mrpt::maps::CColouredOctoMap::getOccupancyThresLog
float getOccupancyThresLog() const override
Definition: CColouredOctoMap.cpp:507
mrpt::maps::CColouredOctoMap::getClampingThresMax
double getClampingThresMax() const override
Definition: CColouredOctoMap.cpp:535
mrpt::maps::CColouredOctoMap::calcNumNodes
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
Definition: CColouredOctoMap.cpp:475
COctoMapBase.h
mrpt::maps::CColouredOctoMap::insertRay
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: CColouredOctoMap.cpp:408
mrpt::maps::CColouredOctoMap::SET
@ SET
Definition: CColouredOctoMap.h:59
mrpt::maps::CColouredOctoMap::getNumLeafNodes
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
Definition: CColouredOctoMap.cpp:479
mrpt::maps::COctoMapBase
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMapBase.h:46
mrpt::maps::CColouredOctoMap::getProbHitLog
float getProbHitLog() const override
Definition: CColouredOctoMap.cpp:515
mrpt::maps::CColouredOctoMap::setClampingThresMax
void setClampingThresMax(double thresProb) override
Definition: CColouredOctoMap.cpp:499
mrpt::maps::CColouredOctoMap::getMetricMin
void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
Definition: CColouredOctoMap.cpp:459
mrpt::maps::CColouredOctoMap::getResolution
double getResolution() const
Definition: CColouredOctoMap.cpp:429
mrpt::maps::CColouredOctoMap::getTreeDepth
unsigned int getTreeDepth() const
Definition: CColouredOctoMap.cpp:433
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:87
mrpt::maps::CColouredOctoMap::CColouredOctoMap
CColouredOctoMap(const double resolution=0.10)
Default constructor.
Definition: CColouredOctoMap.cpp:87
mrpt::maps::CColouredOctoMap::getVoxelColourMethod
TColourUpdate getVoxelColourMethod()
Get the method used to update voxels colour.
Definition: CColouredOctoMap.h:82
mrpt::maps::CColouredOctoMap::updateVoxelColour
void updateVoxelColour(const double x, const double y, const double z, const uint8_t r, const uint8_t g, const uint8_t b)
Manually update the colour of the voxel at (x,y,z)
Definition: CColouredOctoMap.cpp:300
obs_frwds.h
mrpt::maps::CColouredOctoMap::getProbHit
double getProbHit() const override
Definition: CColouredOctoMap.cpp:511
mrpt::maps::CColouredOctoMap::getClampingThresMinLog
float getClampingThresMinLog() const override
Definition: CColouredOctoMap.cpp:531
mrpt::maps::CColouredOctoMap
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: CColouredOctoMap.h:38
mrpt::maps::CColouredOctoMap::getAsOctoMapVoxels
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const override
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object.
Definition: CColouredOctoMap.cpp:322
mrpt::maps::CColouredOctoMap::m_colour_method
TColourUpdate m_colour_method
Definition: CColouredOctoMap.h:168
mrpt::opengl::COctoMapVoxels
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
Definition: COctoMapVoxels.h:72
MAP_DEFINITION_START
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
Definition: TMetricMapTypesRegistry.h:57
mrpt::maps::CColouredOctoMap::isPointWithinOctoMap
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: CColouredOctoMap.cpp:422
mrpt::maps::CColouredOctoMap::setOccupancyThres
void setOccupancyThres(double prob) override
Definition: CColouredOctoMap.cpp:483
MAP_DEFINITION_END
#define MAP_DEFINITION_END(_CLASS_NAME_)
Definition: TMetricMapTypesRegistry.h:67
mrpt::maps::CColouredOctoMap::setProbHit
void setProbHit(double prob) override
Definition: CColouredOctoMap.cpp:487
mrpt::maps::CColouredOctoMap::size
size_t size() const
Definition: CColouredOctoMap.cpp:437
mrpt::maps::CColouredOctoMap::TMapDefinition::likelihoodOpts
mrpt::maps::CColouredOctoMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: CColouredOctoMap.h:93
mrpt::maps::CColouredOctoMap::memoryFullGrid
size_t memoryFullGrid() const
Definition: CColouredOctoMap.cpp:446
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:44
octomap
Definition: CColouredOctoMap.h:16
mrpt::maps::CColouredOctoMap::~CColouredOctoMap
~CColouredOctoMap() override
Destructor.
mrpt::maps::CColouredOctoMap::TMapDefinition::insertionOpts
mrpt::maps::CColouredOctoMap::TInsertionOptions insertionOpts
meters)
Definition: CColouredOctoMap.h:91
mrpt::maps
Definition: CBeacon.h:22
mrpt::maps::CColouredOctoMap::internal_clear
void internal_clear() override
Internal method called by clear()
Definition: CColouredOctoMap.cpp:543
mrpt::maps::CColouredOctoMap::INTEGRATE
@ INTEGRATE
Definition: CColouredOctoMap.h:58
mrpt::maps::CColouredOctoMap::setVoxelColourMethod
void setVoxelColourMethod(TColourUpdate new_method)
Set the method used to update voxels colour.
Definition: CColouredOctoMap.h:76
mrpt::maps::CColouredOctoMap::getPointColour
bool getPointColour(const float x, const float y, const float z, uint8_t &r, uint8_t &g, uint8_t &b) const
Get the RGB colour of a point.
Definition: CColouredOctoMap.cpp:278
mrpt::maps::CColouredOctoMap::AVERAGE
@ AVERAGE
Definition: CColouredOctoMap.h:60
mrpt::maps::CColouredOctoMap::setClampingThresMin
void setClampingThresMin(double thresProb) override
Definition: CColouredOctoMap.cpp:495
mrpt::maps::CColouredOctoMap::TColourUpdate
TColourUpdate
This allows the user to select the desired method to update voxels colour.
Definition: CColouredOctoMap.h:57
mrpt::maps::CColouredOctoMap::getProbMiss
double getProbMiss() const override
Definition: CColouredOctoMap.cpp:519
mrpt::maps::CColouredOctoMap::getClampingThresMin
double getClampingThresMin() const override
Definition: CColouredOctoMap.cpp:527
mrpt::maps::CColouredOctoMap::getProbMissLog
float getProbMissLog() const override
Definition: CColouredOctoMap.cpp:523



Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020