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
16#include <octomap/octomap.h>
19#include <mrpt/obs/obs_frwds.h>
20
22
23namespace 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!
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 */
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
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
191
192 /** Returns a 3D object representing the map.
193 * \sa renderingOptions
194 */
196 {
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
Declares a virtual base class for all metric maps storage classes.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
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 double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
unsigned int getTreeDepth() const
virtual void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
OCTREE octree_t
The type of the octree class in the "octomap" library.
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),...
virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const =0
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object.
void getMetricMin(double &x, double &y, double &z) const
minimum value of the bounding box of all known space in x, y, z
OCTREE_NODE octree_node_t
The type of nodes in the octree, in the "octomap" library.
OCTREE & getOctomap()
Get a reference to the internal octomap object.
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 getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
COctoMapBase(const double resolution=0.10)
Constructor, defines the resolution of the octomap (length of each voxel side)
size_t getNumLeafNodes() const
Traverses the tree to calculate the total number of leaf nodes.
bool getPointOccupancy(const float x, const float y, const float z, double &prob_occupancy) const
Get the occupancy probability [0,1] of a point.
void getMetricSize(double &x, double &y, double &z) const
Size of OcTree (all known space) in meters for x, y and z dimension.
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
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...
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.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
size_t calcNumNodes() const
Traverses the tree to calculate the total number of nodes.
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 getMetricSize(double &x, double &y, double &z)
Size of OcTree (all known space) in meters for x, y and z dimension.
OCTREE m_octomap
The actual octo-map object.
COctoMapBase< OCTREE, OCTREE_NODE > myself_t
The type of this MRPT class.
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 getMetricMax(double &x, double &y, double &z)
maximum value of the bounding box of all known space in x, y, z
TRenderingOptions renderingOptions
TLikelihoodOptions likelihoodOptions
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
Declares a class that represents any robot's observation.
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
static COctoMapVoxelsPtr Create()
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition CPose3D.h:73
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition CStream.h:39
EIGEN_STRONG_INLINE iterator end()
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
struct OPENGL_IMPEXP COctoMapVoxelsPtr
struct OPENGL_IMPEXP CSetOfObjectsPtr
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
unsigned long uint32_t
Definition pstdint.h:216
With this struct options are provided to the observation insertion process.
bool pruning
whether the tree is (losslessly) pruned after insertion (default: true)
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations,...
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.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
mrpt::utils::ignored_copy_ptr< myself_t > m_parent
TInsertionOptions & operator=(const TInsertionOptions &o)
void setProbHit(double prob)
(key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) -...
void setOccupancyThres(double prob)
(key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0...
double maxrange
maximum range for how long individual beams are inserted (default -1: complete beam)
void setClampingThresMax(double thresProb)
(key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor...
void setClampingThresMin(double thresProb)
(key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor...
void setProbMiss(double prob)
(key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds)...
Options used when evaluating "computeObservationLikelihood".
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=1)
TLikelihoodOptions()
Initilization of default parameters.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
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.
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree,...
Lightweight 3D point.
A wrapper class for pointers whose copy operations from other objects of the same type are ignored,...



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 13:56:43 UTC 2023