Main MRPT website > C++ reference for MRPT 1.3.2
OccupancyOcTreeBase.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-2015, 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 #ifndef OCTOMAP_OCCUPANCY_OCTREE_BASE_H
10 #define OCTOMAP_OCCUPANCY_OCTREE_BASE_H
11 
12 // $Id: OccupancyOcTreeBase.h 436 2012-10-15 10:18:16Z ahornung $
13 
14 /**
15 * OctoMap:
16 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
17 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009.
18 * @see http://octomap.sourceforge.net/
19 * License: New BSD License
20 */
21 
22 /*
23  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
24  * All rights reserved.
25  *
26  * Redistribution and use in source and binary forms, with or without
27  * modification, are permitted provided that the following conditions are met:
28  *
29  * * Redistributions of source code must retain the above copyright
30  * notice, this list of conditions and the following disclaimer.
31  * * Redistributions in binary form must reproduce the above copyright
32  * notice, this list of conditions and the following disclaimer in the
33  * documentation and/or other materials provided with the distribution.
34  * * Neither the name of the University of Freiburg nor the names of its
35  * contributors may be used to endorse or promote products derived from
36  * this software without specific prior written permission.
37  *
38  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
39  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
40  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
41  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
42  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
43  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
44  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
45  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
46  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
47  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48  * POSSIBILITY OF SUCH DAMAGE.
49  */
50 
51 #include <list>
52 #include <stdlib.h>
53 
54 #include "octomap_types.h"
55 #include "octomap_utils.h"
56 #include "OcTreeBaseImpl.h"
58 
59 
60 namespace octomap {
61 
62  /**
63  * Base implementation for Occupancy Octrees (e.g. for mapping).
64  * AbstractOccupancyOcTree serves as a common
65  * base interface for all these classes.
66  * Each class used as NODE type needs to be derived from
67  * OccupancyOcTreeNode.
68  *
69  * This tree implementation has a maximum depth of 16.
70  * At a resolution of 1 cm, values have to be < +/- 327.68 meters (2^15)
71  *
72  * This limitation enables the use of an efficient key generation
73  * method which uses the binary representation of the data.
74  *
75  * \note The tree does not save individual points.
76  *
77  * \tparam NODE Node class to be used in tree (usually derived from
78  * OcTreeDataNode)
79  */
80  template <class NODE>
81  class OccupancyOcTreeBase : public OcTreeBaseImpl<NODE,AbstractOccupancyOcTree> {
82 
83  public:
84  /// Default constructor, sets resolution of leafs
86  virtual ~OccupancyOcTreeBase();
87 
88  /// Copy constructor
90 
91  /**
92  * Integrate a Pointcloud (in global reference frame)
93  *
94  * @param scan Pointcloud (measurement endpoints), in global reference frame
95  * @param sensor_origin measurement origin in global reference frame
96  * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
97  * @param pruning whether the tree is (losslessly) pruned after insertion (default: true)
98  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
99  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
100  */
101  virtual void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
102  double maxrange=-1., bool pruning=true, bool lazy_eval = false);
103 
104  template <class SCAN>
105  void insertScan_templ(const SCAN& scan, const octomap::point3d& sensor_origin,
106  double maxrange=-1., bool pruning=true, bool lazy_eval = false);
107 
108  /**
109  * Integrate a 3d scan, transform scan before tree update
110  *
111  * @param scan Pointcloud (measurement endpoints) relative to frame origin
112  * @param sensor_origin origin of sensor relative to frame origin
113  * @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin
114  * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
115  * @param pruning whether the tree is (losslessly) pruned after insertion (default: true)
116  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
117  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
118  */
119  virtual void insertScan(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin,
120  double maxrange=-1., bool pruning = true, bool lazy_eval = false);
121 
122  /**
123  * Insert a 3d scan (given as a ScanNode) into the tree.
124  *
125  * @param scan ScanNode contains Pointcloud data and frame/sensor origin
126  * @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
127  * @param pruning whether the tree is (losslessly) pruned after insertion (default: true)
128  * @param lazy_eval whether the tree is left 'dirty' after the update (default: false).
129  * This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.
130  */
131  virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false);
132 
133  /// for testing only
134  virtual void insertScanNaive(const Pointcloud& pc, const point3d& origin, double maxrange, bool pruning = true, bool lazy_eval = false);
135 
136  /**
137  * Manipulate log_odds value of voxel directly
138  *
139  * @param key OcTreeKey of the NODE that is to be updated
140  * @param log_odds_update value to be added (+) to log_odds value of node
141  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
142  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
143  * @return pointer to the updated NODE
144  */
145  virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false);
146 
147  /**
148  * Manipulate log_odds value of voxel directly.
149  * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
150  *
151  * @param value 3d coordinate of the NODE that is to be updated
152  * @param log_odds_update value to be added (+) to log_odds value of node
153  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
154  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
155  * @return pointer to the updated NODE
156  */
157  virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false);
158 
159  /**
160  * Manipulate log_odds value of voxel directly.
161  * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
162  *
163  * @param x
164  * @param y
165  * @param z
166  * @param log_odds_update value to be added (+) to log_odds value of node
167  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
168  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
169  * @return pointer to the updated NODE
170  */
171  virtual NODE* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false);
172 
173  /**
174  * Integrate occupancy measurement.
175  *
176  * @param key OcTreeKey of the NODE that is to be updated
177  * @param occupied true if the node was measured occupied, else false
178  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
179  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
180  * @return pointer to the updated NODE
181  */
182  virtual NODE* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false);
183 
184  /**
185  * Integrate occupancy measurement.
186  * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
187  *
188  * @param value 3d coordinate of the NODE that is to be updated
189  * @param occupied true if the node was measured occupied, else false
190  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
191  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
192  * @return pointer to the updated NODE
193  */
194  virtual NODE* updateNode(const point3d& value, bool occupied, bool lazy_eval = false);
195 
196  /**
197  * Integrate occupancy measurement.
198  * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
199  *
200  * @param x
201  * @param y
202  * @param z
203  * @param occupied true if the node was measured occupied, else false
204  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
205  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
206  * @return pointer to the updated NODE
207  */
208  virtual NODE* updateNode(double x, double y, double z, bool occupied, bool lazy_eval = false);
209 
210 
211  /**
212  * Creates the maximum likelihood map by calling toMaxLikelihood on all
213  * tree nodes, setting their occupancy to the corresponding occupancy thresholds.
214  * This enables a very efficient compression if you call prune() afterwards.
215  */
216  virtual void toMaxLikelihood();
217 
218  /**
219  * Insert one ray between origin and end into the tree.
220  * integrateMissOnRay() is called for the ray, the end point is updated as occupied.
221  *
222  * @param origin origin of sensor in global coordinates
223  * @param end endpoint of measurement in global coordinates
224  * @param maxrange maximum range after which the raycast should be aborted
225  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
226  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
227  * @return success of operation
228  */
229  virtual bool insertRay(const point3d& origin, const point3d& end, double maxrange=-1.0, bool lazy_eval = false);
230 
231  /**
232  * Performs raycasting in 3d, similar to computeRay().
233  *
234  * A ray is cast from origin with a given direction, the first occupied
235  * cell is returned (as center coordinate). If the starting coordinate is already
236  * occupied in the tree, this coordinate will be returned as a hit.
237  *
238  * @param origin starting coordinate of ray
239  * @param direction A vector pointing in the direction of the raycast. Does not need to be normalized.
240  * @param end returns the center of the cell that was hit by the ray, if successful
241  * @param ignoreUnknownCells whether unknown cells are ignored. If false (default), the raycast aborts when an unkown cell is hit.
242  * @param maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default)
243  * @return whether or not an occupied cell was hit
244  */
245  virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end,
246  bool ignoreUnknownCells=false, double maxRange=-1.0) const;
247 
248  /**
249  * Convenience function to return all occupied nodes in the OcTree.
250  * @note Deprecated, will be removed in the future.
251  * Direcly access the nodes with iterators instead!
252  *
253  * @param node_centers list of occpupied nodes (as point3d)
254  * @param max_depth Depth limit of query. 0 (default): no depth limit
255  */
256  DEPRECATED( void getOccupied(point3d_list& node_centers, unsigned int max_depth = 0) const);
257 
258  /**
259  * Convenience function to return all occupied nodes in the OcTree.
260  * @note Deprecated, will be removed in the future.
261  * Direcly access the nodes with iterators instead! *
262  *
263  * @param occupied_volumes list of occpupied nodes (as point3d and size of the volume)
264  * @param max_depth Depth limit of query. 0 (default): no depth limit
265  */
266  DEPRECATED(void getOccupied(std::list<OcTreeVolume>& occupied_volumes, unsigned int max_depth = 0) const);
267 
268  /**
269  * Traverses the tree and collects all OcTreeVolumes regarded as occupied.
270  * Inner nodes with both occupied and free children are regarded as occupied.
271  * This should be for internal use only, use getOccupied(occupied_volumes) instead.
272  * @note Deprecated, will be removed in the future.
273  * Direcly access the nodes with iterators instead! *
274  *
275  * @param binary_nodes list of binary OcTreeVolumes which are occupied
276  * @param delta_nodes list of delta OcTreeVolumes which are occupied
277  * @param max_depth Depth limit of query. 0 (default): no depth limit
278  */
279  DEPRECATED(void getOccupied(std::list<OcTreeVolume>& binary_nodes, std::list<OcTreeVolume>& delta_nodes,
280  unsigned int max_depth = 0) const);
281 
282 
283  /**
284  * returns occupied leafs within a bounding box defined by min and max.
285  * @note Deprecated, will be removed in the future.
286  * Direcly access the nodes with iterators instead!
287  */
288  DEPRECATED(void getOccupiedLeafsBBX(point3d_list& node_centers, point3d min, point3d max) const);
289 
290  /**
291  * Convenience function to return all free nodes in the OcTree.
292  * @note Deprecated, will be removed in the future.
293  * Direcly access the nodes with iterators instead!
294  *
295  * @param free_volumes list of free nodes (as point3d and size of the volume)
296  * @param max_depth Depth limit of query. 0 (default): no depth limit
297  */
298  DEPRECATED(void getFreespace(std::list<OcTreeVolume>& free_volumes, unsigned int max_depth = 0) const);
299 
300  /**
301  * Traverses the tree and collects all OcTreeVolumes regarded as free.
302  * Inner nodes with both occupied and free children are regarded as occupied.
303  * @note Deprecated, will be removed in the future.
304  * Direcly access the nodes with iterators instead!
305  *
306  * @param binary_nodes list of binary OcTreeVolumes which are free
307  * @param delta_nodes list of delta OcTreeVolumes which are free
308  * @param max_depth Depth limit of query. 0 (default): no depth limit
309  */
310  DEPRECATED(void getFreespace(std::list<OcTreeVolume>& binary_nodes, std::list<OcTreeVolume>& delta_nodes,
311  unsigned int max_depth = 0) const);
312 
313 
314 
315  //-- set BBX limit (limits tree updates to this bounding box
316 
317  /// use or ignore BBX limit (default: ignore)
318  void useBBXLimit(bool enable) { use_bbx_limit = enable; }
319  bool bbxSet() const { return use_bbx_limit; }
320  /// sets the minimum for a query bounding box to use
321  void setBBXMin (point3d& min);
322  /// sets the maximum for a query bounding box to use
323  void setBBXMax (point3d& max);
324  /// @return the currently set minimum for bounding box queries, if set
325  point3d getBBXMin () const { return bbx_min; }
326  /// @return the currently set maximum for bounding box queries, if set
327  point3d getBBXMax () const { return bbx_max; }
328  point3d getBBXBounds () const;
329  point3d getBBXCenter () const;
330  /// @return true if point is in the currently set bounding box
331  bool inBBX(const point3d& p) const;
332  /// @return true if key is in the currently set bounding box
333  bool inBBX(const OcTreeKey& key) const;
334 
335  //-- change detection on occupancy:
336  /// track or ignore changes while inserting scans (default: ignore)
337  void enableChangeDetection(bool enable) { use_change_detection = enable; }
338  /// Reset the set of changed keys. Call this after you obtained all changed nodes.
339  void resetChangeDetection() { changed_keys.clear(); }
340 
341  /**
342  * Iterator to traverse all keys of changed nodes.
343  * you need to enableChangeDetection() first. Here, an OcTreeKey always
344  * refers to a node at the lowest tree level (its size is the minimum tree resolution)
345  */
347 
348  /// Iterator to traverse all keys of changed nodes.
350 
351 
352  /**
353  * Helper for insertScan. Computes all octree nodes affected by the point cloud
354  * integration at once. Here, occupied nodes have a preference over free
355  * ones.
356  *
357  * @param scan point cloud measurement to be integrated
358  * @param origin origin of the sensor for ray casting
359  * @param free_cells keys of nodes to be cleared
360  * @param occupied_cells keys of nodes to be marked occupied
361  * @param maxrange maximum range for raycasting (-1: unlimited)
362  */
363  void computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
364  KeySet& free_cells,
365  KeySet& occupied_cells,
366  double maxrange);
367 
368  template <class POINTCLOUD>
369  void computeUpdate_templ(const POINTCLOUD& scan, const octomap::point3d& origin,
370  KeySet& free_cells,
371  KeySet& occupied_cells,
372  double maxrange);
373 
374 
375  inline void computeUpdate_onePoint(const point3d& p, const octomap::point3d& origin,
376  KeySet& free_cells,
377  KeySet& occupied_cells,
378  double maxrange);
379 
380  // -- I/O -----------------------------------------
381 
382  /**
383  * Reads only the data (=tree structure) from the input stream.
384  * The tree needs to be constructed with the proper header information
385  * beforehand, see readBinary().
386  */
387  std::istream& readBinaryData(std::istream &s);
388 
389  /**
390  * Read node from binary stream (max-likelihood value), recursively
391  * continue with all children.
392  *
393  * This will set the log_odds_occupancy value of
394  * all leaves to either free or occupied.
395  *
396  * @param s
397  * @return
398  */
399  std::istream& readBinaryNode(std::istream &s, NODE* node) const;
400 
401  /**
402  * Write node to binary stream (max-likelihood value),
403  * recursively continue with all children.
404  *
405  * This will discard the log_odds_occupancy value, writing
406  * all leaves as either free or occupied.
407  *
408  * @param s
409  * @param node OcTreeNode to write out, will recurse to all children
410  * @return
411  */
412  std::ostream& writeBinaryNode(std::ostream &s, const NODE* node) const;
413 
414  /**
415  * Writes the data of the tree (without header) to the stream, recursively
416  * calling writeBinaryNode (starting with root)
417  */
418  std::ostream& writeBinaryData(std::ostream &s) const;
419 
420 
421  void calcNumThresholdedNodes(unsigned int& num_thresholded, unsigned int& num_other) const;
422 
423  /**
424  * Updates the occupancy of all inner nodes to reflect their children's occupancy.
425  * If you performed batch-updates with lazy evaluation enabled, you must call this
426  * before any queries to ensure correct multi-resolution behavior.
427  **/
428  void updateInnerOccupancy();
429 
430 
431  /// integrate a "hit" measurement according to the tree's sensor model
432  virtual void integrateHit(NODE* occupancyNode) const;
433  /// integrate a "miss" measurement according to the tree's sensor model
434  virtual void integrateMiss(NODE* occupancyNode) const;
435  // update logodds value of node, given update is added to current value.
436  virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const;
437 
438  /// converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
439  virtual void nodeToMaxLikelihood(NODE* occupancyNode) const;
440  /// converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
441  virtual void nodeToMaxLikelihood(NODE& occupancyNode) const;
442 
443  protected:
444  /// Constructor to enable derived classes to change tree constants.
445  /// This usually requires a re-implementation of some core tree-traversal functions as well!
446  OccupancyOcTreeBase(double resolution, unsigned int tree_depth, unsigned int tree_max_val);
447 
448  /**
449  * Traces a ray from origin to end and updates all voxels on the
450  * way as free. The volume containing "end" is not updated.
451  */
452  inline bool integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval = false);
453 
454 
455  // recursive calls ----------------------------
456 
457  NODE* updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
458  unsigned int depth, const float& log_odds_update, bool lazy_eval = false);
459 
460  void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);
461 
462  void getOccupiedLeafsBBXRecurs( point3d_list& node_centers, unsigned int max_depth, NODE* node,
463  unsigned int depth, const OcTreeKey& parent_key,
464  const OcTreeKey& min, const OcTreeKey& max) const;
465 
466  void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
467 
468  void calcNumThresholdedNodesRecurs (NODE* node,
469  unsigned int& num_thresholded,
470  unsigned int& num_other) const;
471 
472  protected:
473  bool use_bbx_limit; ///< use bounding box for queries (needs to be set)?
478 
480  /// Set of leaf keys (lowest level) which changed since last resetChangeDetection
482 
483 
484  };
485 
486 } // namespace
487 
488 #include "mrpt/otherlibs/octomap/OccupancyOcTreeBase.hxx"
489 
490 #endif
Base implementation for Occupancy Octrees (e.g.
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().
std::ostream & writeBinaryNode(std::ostream &s, const NODE *node) const
Write node to binary stream (max-likelihood value), recursively continue with all children...
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:64
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
point3d getBBXBounds() const
NODE * updateNodeRecurs(NODE *node, bool node_just_created, const OcTreeKey &key, unsigned int depth, const float &log_odds_update, bool lazy_eval=false)
virtual void insertScanNaive(const Pointcloud &pc, const point3d &origin, double maxrange, bool pruning=true, bool lazy_eval=false)
for testing only
KeyBoolMap changed_keys
Set of leaf keys (lowest level) which changed since last resetChangeDetection.
void computeUpdate(const Pointcloud &scan, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
Helper for insertScan.
void setBBXMax(point3d &max)
sets the maximum for a query bounding box to use
void useBBXLimit(bool enable)
use or ignore BBX limit (default: ignore)
std::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:126
void resetChangeDetection()
Reset the set of changed keys. Call this after you obtained all changed nodes.
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.
void enableChangeDetection(bool enable)
track or ignore changes while inserting scans (default: ignore)
void calcNumThresholdedNodesRecurs(NODE *node, unsigned int &num_thresholded, unsigned int &num_other) const
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void calcNumThresholdedNodes(unsigned int &num_thresholded, unsigned int &num_other) const
bool inBBX(const point3d &p) const
void insertScan_templ(const SCAN &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
void updateInnerOccupancy()
Updates the occupancy of all inner nodes to reflect their children's occupancy.
std::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:115
std::ostream & writeBinaryData(std::ostream &s) const
Writes the data of the tree (without header) to the stream, recursively calling writeBinaryNode (star...
virtual void integrateMiss(NODE *occupancyNode) const
integrate a "miss" measurement according to the tree's sensor model
DEPRECATED(void getOccupied(point3d_list &node_centers, unsigned int max_depth=0) const)
Convenience function to return all occupied nodes in the OcTree.
void computeUpdate_templ(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.
KeyBoolMap::const_iterator changedKeysEnd()
Iterator to traverse all keys of changed nodes.
OccupancyOcTreeBase(double resolution)
Default constructor, sets resolution of leafs.
void updateInnerOccupancyRecurs(NODE *node, unsigned int depth)
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
Definition: Pointcloud.h:63
OcTree base class, to be used with with any kind of OcTreeDataNode.
virtual void nodeToMaxLikelihood(NODE *occupancyNode) const
converts the node to the maximum likelihood value according to the tree's parameter for "occupancy" ...
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
Manipulate log_odds value of voxel directly.
void toMaxLikelihoodRecurs(NODE *node, unsigned int depth, unsigned int max_depth)
virtual void updateNodeLogOdds(NODE *occupancyNode, const float &update) const
bool use_bbx_limit
use bounding box for queries (needs to be set)?
void computeUpdate_onePoint(const point3d &p, const octomap::point3d &origin, KeySet &free_cells, KeySet &occupied_cells, double maxrange)
std::list< octomath::Vector3 > point3d_list
Definition: octomap_types.h:68
virtual void toMaxLikelihood()
Creates the maximum likelihood map by calling toMaxLikelihood on all tree nodes, setting their occupa...
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.
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:68
point3d getBBXCenter() const
virtual void insertScan(const Pointcloud &scan, const octomap::point3d &sensor_origin, double maxrange=-1., bool pruning=true, bool lazy_eval=false)
Integrate a Pointcloud (in global reference frame)
virtual void integrateHit(NODE *occupancyNode) const
integrate a "hit" measurement according to the tree's sensor model
A 3D scan as Pointcloud, performed from a Pose6D.
Definition: ScanGraph.h:67
void getOccupiedLeafsBBXRecurs(point3d_list &node_centers, unsigned int max_depth, NODE *node, unsigned int depth, const OcTreeKey &parent_key, const OcTreeKey &min, const OcTreeKey &max) const
std::istream & readBinaryData(std::istream &s)
Reads only the data (=tree structure) from the input stream.
void setBBXMin(point3d &min)
sets the minimum for a query bounding box to use
std::istream & readBinaryNode(std::istream &s, NODE *node) const
Read node from binary stream (max-likelihood value), recursively continue with all children...
KeyBoolMap::const_iterator changedKeysBegin()
Iterator to traverse all keys of changed nodes.
This class represents a three-dimensional vector.
Definition: Vector3.h:65



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Thu Dec 10 00:07:55 UTC 2015