octomap 1.9.8
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
48namespace 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; }
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
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
507
508#endif
OcTree base class, to be used with with any kind of OcTreeDataNode.
Definition: OcTreeBaseImpl.h:75
const unsigned int tree_max_val
Definition: OcTreeBaseImpl.h:546
const iterator end() const
Definition: OcTreeBaseImpl.h:329
const unsigned int tree_depth
Maximum tree depth is fixed to 16 currently.
Definition: OcTreeBaseImpl.h:545
double resolution
in meters
Definition: OcTreeBaseImpl.h:547
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:70
Base implementation for Occupancy Octrees (e.g.
Definition: OccupancyOcTreeBase.h:69
void useBBXLimit(bool enable)
use or ignore BBX limit (default: ignore)
Definition: OccupancyOcTreeBase.h:334
point3d getBBXMax() const
Definition: OccupancyOcTreeBase.h:343
point3d getBBXCenter() const
Definition: OccupancyOcTreeBase.hxx:928
point3d bbx_min
Definition: OccupancyOcTreeBase.h:492
virtual ~OccupancyOcTreeBase()
Definition: OccupancyOcTreeBase.hxx:56
KeyBoolMap::const_iterator changedKeysEnd() const
Iterator to traverse all keys of changed nodes.
Definition: OccupancyOcTreeBase.h:366
void updateInnerOccupancy()
Updates the occupancy of all inner nodes to reflect their children's occupancy.
Definition: OccupancyOcTreeBase.hxx:500
void computeDiscreteUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
Helper for insertPointCloud().
Definition: OccupancyOcTreeBase.hxx:148
virtual void integrateHit(NODE *occupancyNode) const
integrate a "hit" measurement according to the tree's sensor model
Definition: OccupancyOcTreeBase.hxx:1109
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
Definition: OccupancyOcTreeBase.hxx:307
virtual void updateNodeLogOdds(NODE *occupancyNode, const float &update) const
update logodds value of node by adding to the current value.
Definition: OccupancyOcTreeBase.hxx:1097
virtual bool insertRay(const point3d &origin, const point3d &end, double maxrange=-1.0, bool lazy_eval=false)
Insert one ray between origin and end into the tree.
Definition: OccupancyOcTreeBase.hxx:871
virtual void insertPointCloud(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false, bool discretize=false)
Integrate a Pointcloud (in global reference frame), parallelized with OpenMP.
Definition: OccupancyOcTreeBase.hxx:86
point3d getBBXBounds() const
Definition: OccupancyOcTreeBase.hxx:921
std::istream & readBinaryNode(std::istream &s, NODE *node)
Read node from binary stream (max-likelihood value), recursively continue with all children.
Definition: OccupancyOcTreeBase.hxx:960
void enableChangeDetection(bool enable)
track or ignore changes while inserting scans (default: ignore)
Definition: OccupancyOcTreeBase.h:353
bool isChangeDetectionEnabled() const
Definition: OccupancyOcTreeBase.h:354
void computeUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
Helper for insertPointCloud().
Definition: OccupancyOcTreeBase.hxx:169
virtual bool getRayIntersection(const point3d &origin, const point3d &direction, const point3d &center, point3d &intersection, double delta=0.0) const
Retrieves the entry point of a ray into a voxel.
Definition: OccupancyOcTreeBase.hxx:768
virtual bool castRay(const point3d &origin, const point3d &direction, point3d &end, bool ignoreUnknownCells=false, double maxRange=-1.0) const
Performs raycasting in 3d, similar to computeRay().
Definition: OccupancyOcTreeBase.hxx:645
virtual NODE * setNodeValue(const OcTreeKey &key, float log_odds_value, bool lazy_eval=false)
Set log_odds value of voxel to log_odds_value.
Definition: OccupancyOcTreeBase.hxx:273
OcTreeKey bbx_max_key
Definition: OccupancyOcTreeBase.h:495
std::ostream & writeBinaryNode(std::ostream &s, const NODE *node) const
Write node to binary stream (max-likelihood value), recursively continue with all children.
Definition: OccupancyOcTreeBase.hxx:1031
virtual void integrateMiss(NODE *occupancyNode) const
integrate a "miss" measurement according to the tree's sensor model
Definition: OccupancyOcTreeBase.hxx:1114
virtual void insertPointCloudRays(const Pointcloud &scan, const point3d &sensor_origin, double maxrange=-1., bool lazy_eval=false)
Integrate a Pointcloud (in global reference frame), parallelized with OpenMP.
Definition: OccupancyOcTreeBase.hxx:116
KeyBoolMap changed_keys
Set of leaf keys (lowest level) which changed since last resetChangeDetection.
Definition: OccupancyOcTreeBase.h:499
bool use_change_detection
Definition: OccupancyOcTreeBase.h:497
std::ostream & writeBinaryData(std::ostream &s) const
Writes the data of the tree (without header) to the stream, recursively calling writeBinaryNode (star...
Definition: OccupancyOcTreeBase.hxx:952
bool inBBX(const point3d &p) const
Definition: OccupancyOcTreeBase.hxx:908
size_t numChangesDetected() const
Number of changes since last reset.
Definition: OccupancyOcTreeBase.h:369
bool integrateMissOnRay(const point3d &origin, const point3d &end, bool lazy_eval=false)
Traces a ray from origin to end and updates all voxels on the way as free.
Definition: OccupancyOcTreeBase.hxx:857
KeyBoolMap::const_iterator changedKeysBegin() const
Iterator to traverse all keys of changed nodes.
Definition: OccupancyOcTreeBase.h:363
void resetChangeDetection()
Reset the set of changed keys. Call this after you obtained all changed nodes.
Definition: OccupancyOcTreeBase.h:356
bool use_bbx_limit
use bounding box for queries (needs to be set)?
Definition: OccupancyOcTreeBase.h:491
void setBBXMax(const point3d &max)
sets the maximum for a query bounding box to use
Definition: OccupancyOcTreeBase.hxx:899
point3d bbx_max
Definition: OccupancyOcTreeBase.h:493
bool getNormals(const point3d &point, std::vector< point3d > &normals, bool unknownStatus=true) const
Performs a step of the marching cubes surface reconstruction algorithm to retrieve the normal of the ...
Definition: OccupancyOcTreeBase.hxx:557
NODE * updateNodeRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)
Definition: OccupancyOcTreeBase.hxx:373
std::istream & readBinaryData(std::istream &s)
Reads only the data (=complete tree structure) from the input stream.
Definition: OccupancyOcTreeBase.hxx:937
void setBBXMin(const point3d &min)
sets the minimum for a query bounding box to use
Definition: OccupancyOcTreeBase.hxx:891
point3d getBBXMin() const
Definition: OccupancyOcTreeBase.h:341
virtual void toMaxLikelihood()
Creates the maximum likelihood map by calling toMaxLikelihood on all tree nodes, setting their occupa...
Definition: OccupancyOcTreeBase.hxx:524
virtual void nodeToMaxLikelihood(NODE *occupancyNode) const
converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
Definition: OccupancyOcTreeBase.hxx:1119
void updateInnerOccupancyRecurs(NODE *node, unsigned int depth)
Definition: OccupancyOcTreeBase.hxx:506
NODE * setNodeValueRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_value, bool lazy_eval=false)
Definition: OccupancyOcTreeBase.hxx:437
bool bbxSet() const
Definition: OccupancyOcTreeBase.h:335
OcTreeKey bbx_min_key
Definition: OccupancyOcTreeBase.h:494
void toMaxLikelihoodRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
Definition: OccupancyOcTreeBase.hxx:538
OccupancyOcTreeBase(double resolution)
Default constructor, sets resolution of leafs.
Definition: OccupancyOcTreeBase.hxx:42
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
Definition: Pointcloud.h:47
A 3D scan as Pointcloud, performed from a Pose6D.
Definition: ScanGraph.h:52
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
This class represents a three-dimensional vector.
Definition: Vector3.h:50
Namespace the OctoMap library and visualization tools.
unordered_ns::unordered_map< OcTreeKey, bool, OcTreeKey::KeyHash > KeyBoolMap
Data structrure to efficiently track changed nodes as a combination of OcTreeKeys and a bool flag (to...
Definition: OcTreeKey.h:136
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Data structure to efficiently compute the nodes to update from a scan insertion using a hash set.
Definition: OcTreeKey.h:129