509 lines
25 KiB
C++
509 lines
25 KiB
C++
/*
|
|
* OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
|
|
* https://octomap.github.io/
|
|
*
|
|
* Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
|
|
* All rights reserved.
|
|
* License: New BSD
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
* documentation and/or other materials provided with the distribution.
|
|
* * Neither the name of the University of Freiburg nor the names of its
|
|
* contributors may be used to endorse or promote products derived from
|
|
* this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
|
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#ifndef OCTOMAP_OCCUPANCY_OCTREE_BASE_H
|
|
#define OCTOMAP_OCCUPANCY_OCTREE_BASE_H
|
|
|
|
|
|
#include <list>
|
|
#include <stdlib.h>
|
|
#include <vector>
|
|
|
|
#include "octomap_types.h"
|
|
#include "octomap_utils.h"
|
|
#include "OcTreeBaseImpl.h"
|
|
#include "AbstractOccupancyOcTree.h"
|
|
|
|
|
|
namespace octomap {
|
|
|
|
/**
|
|
* Base implementation for Occupancy Octrees (e.g. for mapping).
|
|
* AbstractOccupancyOcTree serves as a common
|
|
* base interface for all these classes.
|
|
* Each class used as NODE type needs to be derived from
|
|
* OccupancyOcTreeNode.
|
|
*
|
|
* This tree implementation has a maximum depth of 16.
|
|
* At a resolution of 1 cm, values have to be < +/- 327.68 meters (2^15)
|
|
*
|
|
* This limitation enables the use of an efficient key generation
|
|
* method which uses the binary representation of the data.
|
|
*
|
|
* \note The tree does not save individual points.
|
|
*
|
|
* \tparam NODE Node class to be used in tree (usually derived from
|
|
* OcTreeDataNode)
|
|
*/
|
|
template <class NODE>
|
|
class OccupancyOcTreeBase : public OcTreeBaseImpl<NODE,AbstractOccupancyOcTree> {
|
|
|
|
public:
|
|
/// Default constructor, sets resolution of leafs
|
|
OccupancyOcTreeBase(double resolution);
|
|
virtual ~OccupancyOcTreeBase();
|
|
|
|
/// Copy constructor
|
|
OccupancyOcTreeBase(const OccupancyOcTreeBase<NODE>& rhs);
|
|
|
|
/**
|
|
* Integrate a Pointcloud (in global reference frame), parallelized with OpenMP.
|
|
* Special care is taken that each voxel
|
|
* in the map is updated only once, and occupied nodes have a preference over free ones.
|
|
* This avoids holes in the floor from mutual deletion and is more efficient than the plain
|
|
* ray insertion in insertPointCloudRays().
|
|
*
|
|
* @note replaces insertScan()
|
|
*
|
|
* @param scan Pointcloud (measurement endpoints), in global reference frame
|
|
* @param sensor_origin measurement origin in global reference frame
|
|
* @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @param discretize whether the scan is discretized first into octree key cells (default: false).
|
|
* This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a potential speedup.*
|
|
*/
|
|
virtual void insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin,
|
|
double maxrange=-1., bool lazy_eval = false, bool discretize = false);
|
|
|
|
/**
|
|
* Integrate a 3d scan (transform scan before tree update), parallelized with OpenMP.
|
|
* Special care is taken that each voxel
|
|
* in the map is updated only once, and occupied nodes have a preference over free ones.
|
|
* This avoids holes in the floor from mutual deletion and is more efficient than the plain
|
|
* ray insertion in insertPointCloudRays().
|
|
*
|
|
* @note replaces insertScan()
|
|
*
|
|
* @param scan Pointcloud (measurement endpoints) relative to frame origin
|
|
* @param sensor_origin origin of sensor relative to frame origin
|
|
* @param frame_origin origin of reference frame, determines transform to be applied to cloud and sensor origin
|
|
* @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @param discretize whether the scan is discretized first into octree key cells (default: false).
|
|
* This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a potential speedup.*
|
|
*/
|
|
virtual void insertPointCloud(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin,
|
|
double maxrange=-1., bool lazy_eval = false, bool discretize = false);
|
|
|
|
/**
|
|
* Insert a 3d scan (given as a ScanNode) into the tree, parallelized with OpenMP.
|
|
*
|
|
* @note replaces insertScan
|
|
*
|
|
* @param scan ScanNode contains Pointcloud data and frame/sensor origin
|
|
* @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
|
|
* @param lazy_eval whether the tree is left 'dirty' after the update (default: false).
|
|
* This speeds up the insertion by not updating inner nodes, but you need to call updateInnerOccupancy() when done.
|
|
* @param discretize whether the scan is discretized first into octree key cells (default: false).
|
|
* This reduces the number of raycasts using computeDiscreteUpdate(), resulting in a potential speedup.
|
|
*/
|
|
virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false, bool discretize = false);
|
|
|
|
/**
|
|
* Integrate a Pointcloud (in global reference frame), parallelized with OpenMP.
|
|
* This function simply inserts all rays of the point clouds as batch operation.
|
|
* Discretization effects can lead to the deletion of occupied space, it is
|
|
* usually recommended to use insertPointCloud() instead.
|
|
*
|
|
* @param scan Pointcloud (measurement endpoints), in global reference frame
|
|
* @param sensor_origin measurement origin in global reference frame
|
|
* @param maxrange maximum range for how long individual beams are inserted (default -1: complete beam)
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
*/
|
|
virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false);
|
|
|
|
/**
|
|
* Set log_odds value of voxel to log_odds_value. This only works if key is at the lowest
|
|
* octree level
|
|
*
|
|
* @param key OcTreeKey of the NODE that is to be updated
|
|
* @param log_odds_value value to be set as the log_odds value of the node
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Set log_odds value of voxel to log_odds_value.
|
|
* Looks up the OcTreeKey corresponding to the coordinate and then calls setNodeValue() with it.
|
|
*
|
|
* @param value 3d coordinate of the NODE that is to be updated
|
|
* @param log_odds_value value to be set as the log_odds value of the node
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Set log_odds value of voxel to log_odds_value.
|
|
* Looks up the OcTreeKey corresponding to the coordinate and then calls setNodeValue() with it.
|
|
*
|
|
* @param x
|
|
* @param y
|
|
* @param z
|
|
* @param log_odds_value value to be set as the log_odds value of the node
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
|
|
* This only works if key is at the lowest octree level
|
|
*
|
|
* @param key OcTreeKey of the NODE that is to be updated
|
|
* @param log_odds_update value to be added (+) to log_odds value of node
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
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).
|
|
* Looks up the OcTreeKey corresponding to the coordinate and then calls updateNode() with it.
|
|
*
|
|
* @param value 3d coordinate of the NODE that is to be updated
|
|
* @param log_odds_update value to be added (+) to log_odds value of node
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Manipulate log_odds value of a voxel by changing it by log_odds_update (relative).
|
|
* Looks up the OcTreeKey corresponding to the coordinate and then calls updateNode() with it.
|
|
*
|
|
* @param x
|
|
* @param y
|
|
* @param z
|
|
* @param log_odds_update value to be added (+) to log_odds value of node
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Integrate occupancy measurement.
|
|
*
|
|
* @param key OcTreeKey of the NODE that is to be updated
|
|
* @param occupied true if the node was measured occupied, else false
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Integrate occupancy measurement.
|
|
* Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
|
|
*
|
|
* @param value 3d coordinate of the NODE that is to be updated
|
|
* @param occupied true if the node was measured occupied, else false
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* updateNode(const point3d& value, bool occupied, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Integrate occupancy measurement.
|
|
* Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
|
|
*
|
|
* @param x
|
|
* @param y
|
|
* @param z
|
|
* @param occupied true if the node was measured occupied, else false
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return pointer to the updated NODE
|
|
*/
|
|
virtual NODE* updateNode(double x, double y, double z, bool occupied, bool lazy_eval = false);
|
|
|
|
|
|
/**
|
|
* Creates the maximum likelihood map by calling toMaxLikelihood on all
|
|
* tree nodes, setting their occupancy to the corresponding occupancy thresholds.
|
|
* This enables a very efficient compression if you call prune() afterwards.
|
|
*/
|
|
virtual void toMaxLikelihood();
|
|
|
|
/**
|
|
* Insert one ray between origin and end into the tree.
|
|
* integrateMissOnRay() is called for the ray, the end point is updated as occupied.
|
|
* It is usually more efficient to insert complete pointcloudsm with insertPointCloud() or
|
|
* insertPointCloudRays().
|
|
*
|
|
* @param origin origin of sensor in global coordinates
|
|
* @param end endpoint of measurement in global coordinates
|
|
* @param maxrange maximum range after which the raycast should be aborted
|
|
* @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
|
|
* This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
|
|
* @return success of operation
|
|
*/
|
|
virtual bool insertRay(const point3d& origin, const point3d& end, double maxrange=-1.0, bool lazy_eval = false);
|
|
|
|
/**
|
|
* Performs raycasting in 3d, similar to computeRay(). Can be called in parallel e.g. with OpenMP
|
|
* for a speedup.
|
|
*
|
|
* A ray is cast from 'origin' with a given direction, the first non-free
|
|
* cell is returned in 'end' (as center coordinate). This could also be the
|
|
* origin node if it is occupied or unknown. castRay() returns true if an occupied node
|
|
* was hit by the raycast. If the raycast returns false you can search() the node at 'end' and
|
|
* see whether it's unknown space.
|
|
*
|
|
*
|
|
* @param[in] origin starting coordinate of ray
|
|
* @param[in] direction A vector pointing in the direction of the raycast (NOT a point in space). Does not need to be normalized.
|
|
* @param[out] end returns the center of the last cell on the ray. If the function returns true, it is occupied.
|
|
* @param[in] ignoreUnknownCells whether unknown cells are ignored (= treated as free). If false (default), the raycast aborts when an unknown cell is hit and returns false.
|
|
* @param[in] maxRange Maximum range after which the raycast is aborted (<= 0: no limit, default)
|
|
* @return true if an occupied cell was hit, false if the maximum range or octree bounds are reached, or if an unknown node was hit.
|
|
*/
|
|
virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end,
|
|
bool ignoreUnknownCells=false, double maxRange=-1.0) const;
|
|
|
|
/**
|
|
* Retrieves the entry point of a ray into a voxel. This is the closest intersection point of the ray
|
|
* originating from origin and a plane of the axis aligned cube.
|
|
*
|
|
* @param[in] origin Starting point of ray
|
|
* @param[in] direction A vector pointing in the direction of the raycast. Does not need to be normalized.
|
|
* @param[in] center The center of the voxel where the ray terminated. This is the output of castRay.
|
|
* @param[out] intersection The entry point of the ray into the voxel, on the voxel surface.
|
|
* @param[in] delta A small increment to avoid ambiguity of beeing exactly on a voxel surface. A positive value will get the point out of the hit voxel, while a negative valuewill get it inside.
|
|
* @return Whether or not an intesection point has been found. Either, the ray never cross the voxel or the ray is exactly parallel to the only surface it intersect.
|
|
*/
|
|
virtual bool getRayIntersection(const point3d& origin, const point3d& direction, const point3d& center,
|
|
point3d& intersection, double delta=0.0) const;
|
|
|
|
/**
|
|
* Performs a step of the marching cubes surface reconstruction algorithm
|
|
* to retrieve the normal of the triangles that fall in the cube
|
|
* formed by the voxels located at the vertex of a given voxel.
|
|
*
|
|
* @param[in] point voxel for which retrieve the normals
|
|
* @param[out] normals normals of the triangles
|
|
* @param[in] unknownStatus consider unknown cells as free (false) or occupied (default, true).
|
|
* @return True if the input voxel is known in the occupancy grid, and false if it is unknown.
|
|
*/
|
|
bool getNormals(const point3d& point, std::vector<point3d>& normals, bool unknownStatus=true) const;
|
|
|
|
//-- set BBX limit (limits tree updates to this bounding box)
|
|
|
|
/// use or ignore BBX limit (default: ignore)
|
|
void useBBXLimit(bool enable) { use_bbx_limit = enable; }
|
|
bool bbxSet() const { return use_bbx_limit; }
|
|
/// sets the minimum for a query bounding box to use
|
|
void setBBXMin (const point3d& min);
|
|
/// sets the maximum for a query bounding box to use
|
|
void setBBXMax (const point3d& max);
|
|
/// @return the currently set minimum for bounding box queries, if set
|
|
point3d getBBXMin () const { return bbx_min; }
|
|
/// @return the currently set maximum for bounding box queries, if set
|
|
point3d getBBXMax () const { return bbx_max; }
|
|
point3d getBBXBounds () const;
|
|
point3d getBBXCenter () const;
|
|
/// @return true if point is in the currently set bounding box
|
|
bool inBBX(const point3d& p) const;
|
|
/// @return true if key is in the currently set bounding box
|
|
bool inBBX(const OcTreeKey& key) const;
|
|
|
|
//-- change detection on occupancy:
|
|
/// track or ignore changes while inserting scans (default: ignore)
|
|
void enableChangeDetection(bool enable) { use_change_detection = enable; }
|
|
bool isChangeDetectionEnabled() const { return use_change_detection; }
|
|
/// Reset the set of changed keys. Call this after you obtained all changed nodes.
|
|
void resetChangeDetection() { changed_keys.clear(); }
|
|
|
|
/**
|
|
* Iterator to traverse all keys of changed nodes.
|
|
* you need to enableChangeDetection() first. Here, an OcTreeKey always
|
|
* refers to a node at the lowest tree level (its size is the minimum tree resolution)
|
|
*/
|
|
KeyBoolMap::const_iterator changedKeysBegin() const {return changed_keys.begin();}
|
|
|
|
/// Iterator to traverse all keys of changed nodes.
|
|
KeyBoolMap::const_iterator changedKeysEnd() const {return changed_keys.end();}
|
|
|
|
/// Number of changes since last reset.
|
|
size_t numChangesDetected() const { return changed_keys.size(); }
|
|
|
|
|
|
/**
|
|
* Helper for insertPointCloud(). Computes all octree nodes affected by the point cloud
|
|
* integration at once. Here, occupied nodes have a preference over free
|
|
* ones.
|
|
*
|
|
* @param scan point cloud measurement to be integrated
|
|
* @param origin origin of the sensor for ray casting
|
|
* @param free_cells keys of nodes to be cleared
|
|
* @param occupied_cells keys of nodes to be marked occupied
|
|
* @param maxrange maximum range for raycasting (-1: unlimited)
|
|
*/
|
|
void computeUpdate(const Pointcloud& scan, const octomap::point3d& origin,
|
|
KeySet& free_cells,
|
|
KeySet& occupied_cells,
|
|
double maxrange);
|
|
|
|
|
|
/**
|
|
* Helper for insertPointCloud(). Computes all octree nodes affected by the point cloud
|
|
* integration at once. Here, occupied nodes have a preference over free
|
|
* ones. This function first discretizes the scan with the octree grid, which results
|
|
* in fewer raycasts (=speedup) but a slightly different result than computeUpdate().
|
|
*
|
|
* @param scan point cloud measurement to be integrated
|
|
* @param origin origin of the sensor for ray casting
|
|
* @param free_cells keys of nodes to be cleared
|
|
* @param occupied_cells keys of nodes to be marked occupied
|
|
* @param maxrange maximum range for raycasting (-1: unlimited)
|
|
*/
|
|
void computeDiscreteUpdate(const Pointcloud& scan, const octomap::point3d& origin,
|
|
KeySet& free_cells,
|
|
KeySet& occupied_cells,
|
|
double maxrange);
|
|
|
|
|
|
// -- I/O -----------------------------------------
|
|
|
|
/**
|
|
* Reads only the data (=complete tree structure) from the input stream.
|
|
* The tree needs to be constructed with the proper header information
|
|
* beforehand, see readBinary().
|
|
*/
|
|
std::istream& readBinaryData(std::istream &s);
|
|
|
|
/**
|
|
* Read node from binary stream (max-likelihood value), recursively
|
|
* continue with all children.
|
|
*
|
|
* This will set the log_odds_occupancy value of
|
|
* all leaves to either free or occupied.
|
|
*/
|
|
std::istream& readBinaryNode(std::istream &s, NODE* node);
|
|
|
|
/**
|
|
* Write node to binary stream (max-likelihood value),
|
|
* recursively continue with all children.
|
|
*
|
|
* This will discard the log_odds_occupancy value, writing
|
|
* all leaves as either free or occupied.
|
|
*
|
|
* @param s
|
|
* @param node OcTreeNode to write out, will recurse to all children
|
|
* @return
|
|
*/
|
|
std::ostream& writeBinaryNode(std::ostream &s, const NODE* node) const;
|
|
|
|
/**
|
|
* Writes the data of the tree (without header) to the stream, recursively
|
|
* calling writeBinaryNode (starting with root)
|
|
*/
|
|
std::ostream& writeBinaryData(std::ostream &s) const;
|
|
|
|
|
|
/**
|
|
* Updates the occupancy of all inner nodes to reflect their children's occupancy.
|
|
* If you performed batch-updates with lazy evaluation enabled, you must call this
|
|
* before any queries to ensure correct multi-resolution behavior.
|
|
**/
|
|
void updateInnerOccupancy();
|
|
|
|
|
|
/// integrate a "hit" measurement according to the tree's sensor model
|
|
virtual void integrateHit(NODE* occupancyNode) const;
|
|
/// integrate a "miss" measurement according to the tree's sensor model
|
|
virtual void integrateMiss(NODE* occupancyNode) const;
|
|
/// update logodds value of node by adding to the current value.
|
|
virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const;
|
|
|
|
/// converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
|
|
virtual void nodeToMaxLikelihood(NODE* occupancyNode) const;
|
|
/// converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
|
|
virtual void nodeToMaxLikelihood(NODE& occupancyNode) const;
|
|
|
|
protected:
|
|
/// Constructor to enable derived classes to change tree constants.
|
|
/// This usually requires a re-implementation of some core tree-traversal functions as well!
|
|
OccupancyOcTreeBase(double resolution, unsigned int tree_depth, unsigned int tree_max_val);
|
|
|
|
/**
|
|
* Traces a ray from origin to end and updates all voxels on the
|
|
* way as free. The volume containing "end" is not updated.
|
|
*/
|
|
inline bool integrateMissOnRay(const point3d& origin, const point3d& end, bool lazy_eval = false);
|
|
|
|
|
|
// recursive calls ----------------------------
|
|
|
|
NODE* updateNodeRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
|
|
unsigned int depth, const float& log_odds_update, bool lazy_eval = false);
|
|
|
|
NODE* setNodeValueRecurs(NODE* node, bool node_just_created, const OcTreeKey& key,
|
|
unsigned int depth, const float& log_odds_value, bool lazy_eval = false);
|
|
|
|
void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);
|
|
|
|
void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
|
|
|
|
|
|
protected:
|
|
bool use_bbx_limit; ///< use bounding box for queries (needs to be set)?
|
|
point3d bbx_min;
|
|
point3d bbx_max;
|
|
OcTreeKey bbx_min_key;
|
|
OcTreeKey bbx_max_key;
|
|
|
|
bool use_change_detection;
|
|
/// Set of leaf keys (lowest level) which changed since last resetChangeDetection
|
|
KeyBoolMap changed_keys;
|
|
|
|
|
|
};
|
|
|
|
} // namespace
|
|
|
|
#include "octomap/OccupancyOcTreeBase.hxx"
|
|
|
|
#endif
|