OccupancyOcTreeBase.h
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * https://octomap.github.io/
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 (const point3d& min);
339  void setBBXMax (const 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
octomap::OccupancyOcTreeBase::bbxSet
bool bbxSet() const
Definition: OccupancyOcTreeBase.h:335
octomap::OccupancyOcTreeBase::updateInnerOccupancy
void updateInnerOccupancy()
octomap::OcTreeBaseImpl
Definition: OcTreeBaseImpl.h:75
octomap::OccupancyOcTreeBase::insertRay
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
octomap::OccupancyOcTreeBase::nodeToMaxLikelihood
virtual void nodeToMaxLikelihood(NODE *occupancyNode) const
converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
octomap::OccupancyOcTreeBase
Definition: OccupancyOcTreeBase.h:69
octomap::OccupancyOcTreeBase::getBBXMax
point3d getBBXMax() const
Definition: OccupancyOcTreeBase.h:343
octomap::OccupancyOcTreeBase::updateNode
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
octomap::OccupancyOcTreeBase::setNodeValue
virtual NODE * setNodeValue(const OcTreeKey &key, float log_odds_value, bool lazy_eval=false)
octomap::OccupancyOcTreeBase::castRay
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
OcTreeBaseImpl.h
octomap::Pointcloud
Definition: Pointcloud.h:47
octomap::OccupancyOcTreeBase::changedKeysBegin
KeyBoolMap::const_iterator changedKeysBegin() const
Definition: OccupancyOcTreeBase.h:363
octomap::OccupancyOcTreeBase::toMaxLikelihood
virtual void toMaxLikelihood()
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::tree_max_val
const unsigned int tree_max_val
Definition: OcTreeBaseImpl.h:546
octomap::OccupancyOcTreeBase::isChangeDetectionEnabled
bool isChangeDetectionEnabled() const
Definition: OccupancyOcTreeBase.h:354
octomap::OccupancyOcTreeBase::~OccupancyOcTreeBase
virtual ~OccupancyOcTreeBase()
octomap::OccupancyOcTreeBase::updateNodeLogOdds
virtual void updateNodeLogOdds(NODE *occupancyNode, const float &update) const
update logodds value of node by adding to the current value.
octomap::OccupancyOcTreeBase::toMaxLikelihoodRecurs
void toMaxLikelihoodRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
octomap::KeyBoolMap
unordered_ns::unordered_map< OcTreeKey, bool, OcTreeKey::KeyHash > KeyBoolMap
Definition: OcTreeKey.h:136
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::tree_depth
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
Definition: OcTreeBaseImpl.h:545
octomap::OccupancyOcTreeBase::OccupancyOcTreeBase
OccupancyOcTreeBase(double resolution)
Default constructor, sets resolution of leafs.
octomap::OccupancyOcTreeBase::updateInnerOccupancyRecurs
void updateInnerOccupancyRecurs(NODE *node, unsigned int depth)
octomap::OccupancyOcTreeBase::getBBXBounds
point3d getBBXBounds() const
octomap::OccupancyOcTreeBase::resetChangeDetection
void resetChangeDetection()
Reset the set of changed keys. Call this after you obtained all changed nodes.
Definition: OccupancyOcTreeBase.h:356
octomap::OccupancyOcTreeBase::bbx_min
point3d bbx_min
Definition: OccupancyOcTreeBase.h:492
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::OccupancyOcTreeBase::bbx_min_key
OcTreeKey bbx_min_key
Definition: OccupancyOcTreeBase.h:494
octomap::OccupancyOcTreeBase::insertPointCloudRays
virtual void insertPointCloudRays(const Pointcloud &scan, const point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false)
AbstractOccupancyOcTree.h
octomap::KeySet
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Definition: OcTreeKey.h:129
octomap::OccupancyOcTreeBase::computeUpdate
void computeUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
octomap::OccupancyOcTreeBase::getBBXMin
point3d getBBXMin() const
Definition: OccupancyOcTreeBase.h:341
octomap::ScanNode
Definition: ScanGraph.h:52
octomap::OccupancyOcTreeBase::updateNodeRecurs
NODE * updateNodeRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)
octomap::OccupancyOcTreeBase::integrateMissOnRay
bool integrateMissOnRay(const point3d &origin, const point3d &end, bool lazy_eval=false)
octomap::OccupancyOcTreeBase::bbx_max_key
OcTreeKey bbx_max_key
Definition: OccupancyOcTreeBase.h:495
octomap::OccupancyOcTreeBase::setBBXMax
void setBBXMax(const point3d &max)
sets the maximum for a query bounding box to use
octomap::OccupancyOcTreeBase::setNodeValueRecurs
NODE * setNodeValueRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_value, bool lazy_eval=false)
octomap::OcTreeKey
Definition: OcTreeKey.h:70
octomap_types.h
octomap::OccupancyOcTreeBase::use_change_detection
bool use_change_detection
Definition: OccupancyOcTreeBase.h:497
octomap::OccupancyOcTreeBase::writeBinaryNode
std::ostream & writeBinaryNode(std::ostream &s, const NODE *node) const
octomap::OccupancyOcTreeBase::getBBXCenter
point3d getBBXCenter() const
octomap::OccupancyOcTreeBase::readBinaryData
std::istream & readBinaryData(std::istream &s)
octomap::OccupancyOcTreeBase::changedKeysEnd
KeyBoolMap::const_iterator changedKeysEnd() const
Iterator to traverse all keys of changed nodes.
Definition: OccupancyOcTreeBase.h:366
octomap::OccupancyOcTreeBase::getRayIntersection
virtual bool getRayIntersection(const point3d &origin, const point3d &direction, const point3d &center, point3d &intersection, double delta=0.0) const
octomap::OccupancyOcTreeBase::enableChangeDetection
void enableChangeDetection(bool enable)
track or ignore changes while inserting scans (default: ignore)
Definition: OccupancyOcTreeBase.h:353
octomap::OccupancyOcTreeBase::numChangesDetected
size_t numChangesDetected() const
Number of changes since last reset.
Definition: OccupancyOcTreeBase.h:369
octomap::OccupancyOcTreeBase::bbx_max
point3d bbx_max
Definition: OccupancyOcTreeBase.h:493
octomap::OccupancyOcTreeBase::computeDiscreteUpdate
void computeDiscreteUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
octomap::OccupancyOcTreeBase::readBinaryNode
std::istream & readBinaryNode(std::istream &s, NODE *node)
octomap::OccupancyOcTreeBase::inBBX
bool inBBX(const point3d &p) const
octomap::OccupancyOcTreeBase::use_bbx_limit
bool use_bbx_limit
use bounding box for queries (needs to be set)?
Definition: OccupancyOcTreeBase.h:491
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::end
const iterator end() const
Definition: OcTreeBaseImpl.h:329
octomap::OccupancyOcTreeBase::setBBXMin
void setBBXMin(const point3d &min)
sets the minimum for a query bounding box to use
octomap::OccupancyOcTreeBase::integrateMiss
virtual void integrateMiss(NODE *occupancyNode) const
integrate a "miss" measurement according to the tree's sensor model
octomap
octomath::Pose6D
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
octomap::OccupancyOcTreeBase::useBBXLimit
void useBBXLimit(bool enable)
use or ignore BBX limit (default: ignore)
Definition: OccupancyOcTreeBase.h:334
octomap::OccupancyOcTreeBase::integrateHit
virtual void integrateHit(NODE *occupancyNode) const
integrate a "hit" measurement according to the tree's sensor model
octomap_utils.h
octomap::OccupancyOcTreeBase::getNormals
bool getNormals(const point3d &point, std::vector< point3d > &normals, bool unknownStatus=true) const
octomap::OcTreeBaseImpl< NODE, AbstractOccupancyOcTree >::resolution
double resolution
in meters
Definition: OcTreeBaseImpl.h:547
octomap::OccupancyOcTreeBase::changed_keys
KeyBoolMap changed_keys
Set of leaf keys (lowest level) which changed since last resetChangeDetection.
Definition: OccupancyOcTreeBase.h:499
octomap::OccupancyOcTreeBase::insertPointCloud
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
octomap::OccupancyOcTreeBase::writeBinaryData
std::ostream & writeBinaryData(std::ostream &s) const


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Tue Dec 12 2023 03:39:40