OccupancyOcTreeBase.h
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #ifndef OCTOMAP_OCCUPANCY_OCTREE_BASE_H
35 #define OCTOMAP_OCCUPANCY_OCTREE_BASE_H
36 
37 
38 #include <list>
39 #include <stdlib.h>
40 #include <vector>
41 
42 #include "octomap_types.h"
43 #include "octomap_utils.h"
44 #include "OcTreeBaseImpl.h"
46 
47 
48 namespace octomap {
49 
68  template <class NODE>
69  class OccupancyOcTreeBase : public OcTreeBaseImpl<NODE,AbstractOccupancyOcTree> {
70 
71  public:
74  virtual ~OccupancyOcTreeBase();
75 
78 
96  virtual void insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin,
97  double maxrange=-1., bool lazy_eval = false, bool discretize = false);
98 
117  virtual void insertPointCloud(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin,
118  double maxrange=-1., bool lazy_eval = false, bool discretize = false);
119 
132  virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false, bool discretize = false);
133 
146  virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false);
147 
158  virtual NODE* setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval = false);
159 
170  virtual NODE* setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval = false);
171 
184  virtual NODE* setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval = false);
185 
196  virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false);
197 
208  virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false);
209 
222  virtual NODE* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false);
223 
233  virtual NODE* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false);
234 
245  virtual NODE* updateNode(const point3d& value, bool occupied, bool lazy_eval = false);
246 
259  virtual NODE* updateNode(double x, double y, double z, bool occupied, bool lazy_eval = false);
260 
261 
267  virtual void toMaxLikelihood();
268 
282  virtual bool insertRay(const point3d& origin, const point3d& end, double maxrange=-1.0, bool lazy_eval = false);
283 
302  virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end,
303  bool ignoreUnknownCells=false, double maxRange=-1.0) const;
304 
316  virtual bool getRayIntersection(const point3d& origin, const point3d& direction, const point3d& center,
317  point3d& intersection, double delta=0.0) const;
318 
329  bool getNormals(const point3d& point, std::vector<point3d>& normals, bool unknownStatus=true) const;
330 
331  //-- set BBX limit (limits tree updates to this bounding box)
332 
334  void useBBXLimit(bool enable) { use_bbx_limit = enable; }
335  bool bbxSet() const { return use_bbx_limit; }
337  void setBBXMin (point3d& min);
339  void setBBXMax (point3d& max);
341  point3d getBBXMin () const { return bbx_min; }
343  point3d getBBXMax () const { return bbx_max; }
344  point3d getBBXBounds () const;
345  point3d getBBXCenter () const;
347  bool inBBX(const point3d& p) const;
349  bool inBBX(const OcTreeKey& key) const;
350 
351  //-- change detection on occupancy:
353  void enableChangeDetection(bool enable) { use_change_detection = enable; }
356  void resetChangeDetection() { changed_keys.clear(); }
357 
363  KeyBoolMap::const_iterator changedKeysBegin() const {return changed_keys.begin();}
364 
366  KeyBoolMap::const_iterator changedKeysEnd() const {return changed_keys.end();}
367 
369  size_t numChangesDetected() const { return changed_keys.size(); }
370 
371 
383  void computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
384  KeySet& free_cells,
385  KeySet& occupied_cells,
386  double maxrange);
387 
388 
401  void computeDiscreteUpdate(const Pointcloud& scan, const octomap::point3d& origin,
402  KeySet& free_cells,
403  KeySet& occupied_cells,
404  double maxrange);
405 
406 
407  // -- I/O -----------------------------------------
408 
414  std::istream& readBinaryData(std::istream &s);
415 
423  std::istream& readBinaryNode(std::istream &s, NODE* node);
424 
436  std::ostream& writeBinaryNode(std::ostream &s, const NODE* node) const;
437 
442  std::ostream& writeBinaryData(std::ostream &s) const;
443 
444 
450  void updateInnerOccupancy();
451 
452 
454  virtual void integrateHit(NODE* occupancyNode) const;
456  virtual void integrateMiss(NODE* occupancyNode) const;
458  virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const;
459 
461  virtual void nodeToMaxLikelihood(NODE* occupancyNode) const;
463  virtual void nodeToMaxLikelihood(NODE& occupancyNode) const;
464 
465  protected:
468  OccupancyOcTreeBase(double resolution, unsigned int tree_depth, unsigned int tree_max_val);
469 
474  inline bool integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval = false);
475 
476 
477  // recursive calls ----------------------------
478 
479  NODE* updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
480  unsigned int depth, const float& log_odds_update, bool lazy_eval = false);
481 
482  NODE* setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
483  unsigned int depth, const float& log_odds_value, bool lazy_eval = false);
484 
485  void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);
486 
487  void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
488 
489 
490  protected:
496 
500 
501 
502  };
503 
504 } // namespace
505 
506 #include "octomap/OccupancyOcTreeBase.hxx"
507 
508 #endif
void updateInnerOccupancyRecurs(NODE *node, unsigned int depth)
bool inBBX(const point3d &p) const
void computeDiscreteUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
bool integrateMissOnRay(const point3d &origin, const point3d &end, bool lazy_eval=false)
size_t numChangesDetected() const
Number of changes since last reset.
std::istream & readBinaryData(std::istream &s)
unordered_ns::unordered_map< OcTreeKey, bool, OcTreeKey::KeyHash > KeyBoolMap
Definition: OcTreeKey.h:136
KeyBoolMap::const_iterator changedKeysEnd() const
Iterator to traverse all keys of changed nodes.
OccupancyOcTreeBase(double resolution)
Default constructor, sets resolution of leafs.
void setBBXMax(point3d &max)
sets the maximum for a query bounding box to use
virtual NODE * setNodeValue(const OcTreeKey &key, float log_odds_value, bool lazy_eval=false)
virtual void integrateHit(NODE *occupancyNode) const
integrate a "hit" measurement according to the tree&#39;s sensor model
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
point3d getBBXBounds() const
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
std::istream & readBinaryNode(std::istream &s, NODE *node)
KeyBoolMap::const_iterator changedKeysBegin() const
bool use_bbx_limit
use bounding box for queries (needs to be set)?
virtual void updateNodeLogOdds(NODE *occupancyNode, const float &update) const
update logodds value of node by adding to the current value.
virtual void insertPointCloudRays(const Pointcloud &scan, const point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false)
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Definition: OcTreeKey.h:129
std::ostream & writeBinaryData(std::ostream &s) const
NODE * setNodeValueRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_value, bool lazy_eval=false)
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
void useBBXLimit(bool enable)
use or ignore BBX limit (default: ignore)
NODE * updateNodeRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)
bool getNormals(const point3d &point, std::vector< point3d > &normals, bool unknownStatus=true) const
This class represents a three-dimensional vector.
Definition: Vector3.h:50
virtual void nodeToMaxLikelihood(NODE *occupancyNode) const
converts the node to the maximum likelihood value according to the tree&#39;s parameter for "occupancy" ...
void setBBXMin(point3d &min)
sets the minimum for a query bounding box to use
virtual void toMaxLikelihood()
void computeUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
KeyBoolMap changed_keys
Set of leaf keys (lowest level) which changed since last resetChangeDetection.
virtual bool getRayIntersection(const point3d &origin, const point3d &direction, const point3d &center, point3d &intersection, double delta=0.0) const
void resetChangeDetection()
Reset the set of changed keys. Call this after you obtained all changed nodes.
virtual void integrateMiss(NODE *occupancyNode) const
integrate a "miss" measurement according to the tree&#39;s sensor model
void enableChangeDetection(bool enable)
track or ignore changes while inserting scans (default: ignore)
void toMaxLikelihoodRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
std::ostream & writeBinaryNode(std::ostream &s, const NODE *node) const
point3d getBBXCenter() const


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06