Main MRPT website > C++ reference for MRPT 1.5.9
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-2017, 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>
17 #include <mrpt/obs/obs_frwds.h>
19 #include <mrpt/utils/pimpl.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_t,class octree_node_t>
44  {
45  public:
46  typedef COctoMapBase<octree_t, octree_node_t> myself_t; //!< The type of this MRPT class
47 
48  /** Constructor, defines the resolution of the octomap (length of each voxel side) */
50  virtual ~COctoMapBase() { }
51 
52  /** Get a reference to the internal octomap object. Example:
53  * \code
54  * mrpt::maps::COctoMap map;
55  * octomap::OcTree &om = map.getOctomap<octomap::OcTree>();
56  * \endcode
57  */
58  template <class OCTOMAP_CLASS>
59  inline OCTOMAP_CLASS & getOctomap() { return *m_octomap.ptr.get(); }
60 
61  /** With this struct options are provided to the observation insertion process.
62  * \sa CObservation::insertObservationInto()
63  */
65  {
66  /** Initilization of default parameters */
67  TInsertionOptions( myself_t &parent );
68 
69  TInsertionOptions(); //!< Especial constructor, not attached to a real COctoMap object: used only in limited situations, since get*() methods don't work, etc.
71  {
72  // Copy all but the m_parent pointer!
73  maxrange = o.maxrange;
74  pruning = o.pruning;
75  const bool o_has_parent = o.m_parent.get()!=NULL;
76  setOccupancyThres( o_has_parent ? o.getOccupancyThres() : o.occupancyThres );
77  setProbHit( o_has_parent ? o.getProbHit() : o.probHit );
78  setProbMiss( o_has_parent ? o.getProbMiss() : o.probMiss );
81  return *this;
82  }
83 
84  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
85  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
86 
87  double maxrange; //!< maximum range for how long individual beams are inserted (default -1: complete beam)
88  bool pruning; //!< whether the tree is (losslessly) pruned after insertion (default: true)
89 
90  /// (key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0.5)
91  void setOccupancyThres(double prob) { if(m_parent.get()) m_parent->setOccupancyThres(prob); }
92  /// (key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) - sensor model (Default=0.7)
93  void setProbHit(double prob) { if(m_parent.get()) m_parent->setProbHit(prob); }
94  /// (key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds) - sensor model (Default=0.4)
95  void setProbMiss(double prob) { if(m_parent.get()) m_parent->setProbMiss(prob); }
96  /// (key name in .ini files: "clampingThresMin")sets the minimum threshold for occupancy clamping (sensor model) (Default=0.1192, -2 in log odds)
97  void setClampingThresMin(double thresProb) { if(m_parent.get()) m_parent->setClampingThresMin(thresProb); }
98  /// (key name in .ini files: "clampingThresMax")sets the maximum threshold for occupancy clamping (sensor model) (Default=0.971, 3.5 in log odds)
99  void setClampingThresMax(double thresProb) { if(m_parent.get()) m_parent->setClampingThresMax(thresProb); }
100 
101  /// @return threshold (probability) for occupancy - sensor model
102  double getOccupancyThres() const { if(m_parent.get()) return m_parent->getOccupancyThres(); else return this->occupancyThres; }
103  /// @return threshold (logodds) for occupancy - sensor model
104  float getOccupancyThresLog() const { return m_parent->getOccupancyThresLog() ; }
105 
106  /// @return probablility for a "hit" in the sensor model (probability)
107  double getProbHit() const { if(m_parent.get()) return m_parent->getProbHit(); else return this->probHit; }
108  /// @return probablility for a "hit" in the sensor model (logodds)
109  float getProbHitLog() const { return m_parent->getProbHitLog(); }
110  /// @return probablility for a "miss" in the sensor model (probability)
111  double getProbMiss() const { if(m_parent.get()) return m_parent->getProbMiss(); else return this->probMiss; }
112  /// @return probablility for a "miss" in the sensor model (logodds)
113  float getProbMissLog() const { return m_parent->getProbMissLog(); }
114 
115  /// @return minimum threshold for occupancy clamping in the sensor model (probability)
116  double getClampingThresMin() const { if(m_parent.get()) return m_parent->getClampingThresMin(); else return this->clampingThresMin; }
117  /// @return minimum threshold for occupancy clamping in the sensor model (logodds)
118  float getClampingThresMinLog() const { return m_parent->getClampingThresMinLog(); }
119  /// @return maximum threshold for occupancy clamping in the sensor model (probability)
120  double getClampingThresMax() const { if(m_parent.get()) return m_parent->getClampingThresMax(); else return this->clampingThresMax; }
121  /// @return maximum threshold for occupancy clamping in the sensor model (logodds)
122  float getClampingThresMaxLog() const { return m_parent->getClampingThresMaxLog(); }
123 
124  private:
126 
127  double occupancyThres; // sets the threshold for occupancy (sensor model) (Default=0.5)
128  double probHit; // sets the probablility for a "hit" (will be converted to logodds) - sensor model (Default=0.7)
129  double probMiss; // sets the probablility for a "miss" (will be converted to logodds) - sensor model (Default=0.4)
130  double clampingThresMin; // sets the minimum threshold for occupancy clamping (sensor model) (Default=0.1192, -2 in log odds)
131  double clampingThresMax; // sets the maximum threshold for occupancy clamping (sensor model) (Default=0.971, 3.5 in log odds)
132  };
133 
134  TInsertionOptions insertionOptions; //!< The options used when inserting observations in the map
135 
136  /** Options used when evaluating "computeObservationLikelihood"
137  * \sa CObservation::computeObservationLikelihood
138  */
140  {
141  /** Initilization of default parameters
142  */
144  virtual ~TLikelihoodOptions() {}
145 
146  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
147  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
148 
149  void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream
150  void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream
151 
152  uint32_t decimation; //!< Speed up the likelihood computation by considering only one out of N rays (default=1)
153  };
154 
155  TLikelihoodOptions likelihoodOptions;
156 
157  virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
158 
159  /** Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels */
161  {
162  bool generateGridLines; //!< Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
163 
164  bool generateOccupiedVoxels; //!< Generate voxels for the occupied volumes (Default=true)
165  bool visibleOccupiedVoxels; //!< Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
166 
167  bool generateFreeVoxels; //!< Generate voxels for the freespace (Default=true)
168  bool visibleFreeVoxels; //!< Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
169 
171  generateGridLines (false),
172  generateOccupiedVoxels (true),
173  visibleOccupiedVoxels (true),
174  generateFreeVoxels (true),
175  visibleFreeVoxels (true)
176  { }
177 
178  void writeToStream(mrpt::utils::CStream &out) const; //!< Binary dump to stream
179  void readFromStream(mrpt::utils::CStream &in); //!< Binary dump to stream
180  };
181 
182  TRenderingOptions renderingOptions;
183 
184  /** Returns a 3D object representing the map.
185  * \sa renderingOptions
186  */
187  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const MRPT_OVERRIDE
188  {
189  mrpt::opengl::COctoMapVoxelsPtr gl_obj = mrpt::opengl::COctoMapVoxels::Create();
190  this->getAsOctoMapVoxels(*gl_obj);
191  outObj->insert(gl_obj);
192  }
193 
194  /** Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object.
195  * \sa renderingOptions
196  */
197  virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const = 0;
198 
199  /** Get the occupancy probability [0,1] of a point
200  * \return false if the point is not mapped, in which case the returned "prob" is undefined. */
201  bool getPointOccupancy(const float x,const float y,const float z, double &prob_occupancy) const;
202 
203  /** 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.
204  * Insertion parameters can be found in \a insertionOptions.
205  * \sa The generic observation insertion method CMetricMap::insertObservation()
206  */
207  void insertPointCloud(const CPointsMap &ptMap, const float sensor_x,const float sensor_y,const float sensor_z);
208 
209  /** Performs raycasting in 3d, similar to computeRay().
210  *
211  * A ray is cast from origin with a given direction, the first occupied
212  * cell is returned (as center coordinate). If the starting coordinate is already
213  * occupied in the tree, this coordinate will be returned as a hit.
214  *
215  * @param origin starting coordinate of ray
216  * @param direction A vector pointing in the direction of the raycast. Does not need to be normalized.
217  * @param end returns the center of the cell that was hit by the ray, if successful
218  * @param ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit.
219  * @param maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default)
220  * @return whether or not an occupied cell was hit
221  */
222  bool castRay(
223  const mrpt::math::TPoint3D & origin,
224  const mrpt::math::TPoint3D & direction,
226  bool ignoreUnknownCells=false,
227  double maxRange=-1.0) const;
228 
229  virtual void setOccupancyThres(double prob) = 0;
230  virtual void setProbHit(double prob) = 0;
231  virtual void setProbMiss(double prob) = 0;
232  virtual void setClampingThresMin(double thresProb) = 0;
233  virtual void setClampingThresMax(double thresProb) = 0;
234  virtual double getOccupancyThres() const = 0;
235  virtual float getOccupancyThresLog() const = 0;
236  virtual double getProbHit() const = 0;
237  virtual float getProbHitLog() const = 0;
238  virtual double getProbMiss() const = 0;
239  virtual float getProbMissLog() const = 0;
240  virtual double getClampingThresMin() const = 0;
241  virtual float getClampingThresMinLog() const = 0;
242  virtual double getClampingThresMax() const = 0;
243  virtual float getClampingThresMaxLog() const = 0;
244 
245  protected:
246  /** Builds the list of 3D points in global coordinates for a generic observation. Used for both, insertObservation() and computeLikelihood().
247  * \param[out] point3d_sensorPt Is a pointer to a "point3D".
248  * \param[out] ptr_scan Is in fact a pointer to "octomap::Pointcloud". Not declared as such to avoid headers dependencies in user code.
249  * \return false if the observation kind is not applicable.
250  */
251  template <class octomap_point3d, class octomap_pointcloud>
252  bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose, octomap_point3d &sensorPt, octomap_pointcloud &scan) const;
253 
254  PIMPL_DECLARE_TYPE(octree_t, m_octomap); //!< The actual octo-map object.
255 
256  private:
257  // See docs in base class
259 
260  }; // End of class def.
261  } // End of namespace
262 } // End of namespace
263 
264 #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.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Options for the conversion of a mrpt::maps::COctoMap into a mrpt::opengl::COctoMapVoxels.
virtual void setProbMiss(double prob)=0
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=1) ...
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
virtual double getOccupancyThres() const =0
virtual double getClampingThresMin() const =0
GLdouble GLdouble z
Definition: glext.h:3734
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
TRenderingOptions renderingOptions
PIMPL_DECLARE_TYPE(octree_t, m_octomap)
The actual octo-map object.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
COctoMapBase()
Constructor, defines the resolution of the octomap (length of each voxel side)
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...
virtual void setProbHit(double prob)=0
virtual double getClampingThresMax() const =0
void setOccupancyThres(double prob)
(key name in .ini files: "occupancyThres") sets the threshold for occupancy (sensor model) (Default=0...
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
virtual float getProbHitLog() const =0
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
TInsertionOptions & operator=(const TInsertionOptions &o)
double maxrange
maximum range for how long individual beams are inserted (default -1: complete beam) ...
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
This class allows loading and storing values and vectors of different types from a configuration text...
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).
virtual void setClampingThresMin(double thresProb)=0
TLikelihoodOptions()
Initilization of default parameters.
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...
void writeToStream(mrpt::utils::CStream &out) const
Binary dump to stream.
TLikelihoodOptions likelihoodOptions
OCTOMAP_CLASS & getOctomap()
Get a reference to the internal octomap object.
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.
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true) ...
GLuint GLuint end
Definition: glext.h:3512
bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose, octomap_point3d &sensorPt, octomap_pointcloud &scan) const
Builds the list of 3D points in global coordinates for a generic observation.
COctoMapBase< octree_t, octree_node_t > myself_t
The type of this MRPT class.
TInsertionOptions()
Especial constructor, not attached to a real COctoMap object: used only in limited situations...
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
GLsizei const GLchar ** string
Definition: glext.h:3919
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().
mrpt::utils::ignored_copy_ptr< myself_t > m_parent
bool pruning
whether the tree is (losslessly) pruned after insertion (default: true)
virtual float getProbMissLog() const =0
void setProbMiss(double prob)
(key name in .ini files: "probMiss")sets the probablility for a "miss" (will be converted to logodds)...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual float getClampingThresMaxLog() const =0
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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.
Declares a class that represents any robot&#39;s observation.
virtual double getProbMiss() const =0
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...
GLuint in
Definition: glext.h:6301
void readFromStream(mrpt::utils::CStream &in)
Binary dump to stream.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
virtual void setOccupancyThres(double prob)=0
virtual float getClampingThresMinLog() const =0
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
GLenum GLint GLint y
Definition: glext.h:3516
void setProbHit(double prob)
(key name in .ini files: "probHit")sets the probablility for a "hit" (will be converted to logodds) -...
virtual double getProbHit() const =0
virtual float getOccupancyThresLog() const =0
With this struct options are provided to the observation insertion process.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
GLenum GLint x
Definition: glext.h:3516
static COctoMapVoxelsPtr Create()
Lightweight 3D point.
Options used when evaluating "computeObservationLikelihood".
virtual void setClampingThresMax(double thresProb)=0
unsigned __int32 uint32_t
Definition: rptypes.h:49
A wrapper class for pointers whose copy operations from other objects of the same type are ignored...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
virtual void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const =0
Builds a renderizable representation of the octomap as a mrpt::opengl::COctoMapVoxels object...



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020