MRPT  2.0.1
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,
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
unsigned int getTreeDepth() const
void setClampingThresMax(double thresProb) override
double getProbMiss() const override
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
float getClampingThresMinLog() const override
double getProbHit() const override
mrpt::maps::CColouredOctoMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
double getClampingThresMin() const override
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 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)...
void internal_clear() override
Internal method called by clear()
void setClampingThresMin(double thresProb) override
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).
void setVoxelColourMethod(TColourUpdate new_method)
Set the method used to update voxels colour.
mrpt::maps::CColouredOctoMap::TInsertionOptions insertionOpts
meters)
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)
double getOccupancyThres() const override
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.
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
TColourUpdate
This allows the user to select the desired method to update voxels colour.
void setProbMiss(double prob) override
void setProbHit(double prob) override
float getClampingThresMaxLog() const override
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
TColourUpdate getVoxelColourMethod()
Get the method used to update voxels colour.
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
float getProbMissLog() const override
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose) override
Internal method called by insertObservation()
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
float getOccupancyThresLog() const override
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
void setOccupancyThres(double prob) override
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const override
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object...
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
#define MAP_DEFINITION_END(_CLASS_NAME_)
void getMetricMin(double &x, double &y, double &z)
minimum value of the bounding box of all known space in x, y, z
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.
float getProbHitLog() const override
double getClampingThresMax() const override
~CColouredOctoMap() override
Destructor.
CColouredOctoMap(const double resolution=0.10)
Default constructor.



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