.. _program_listing_file__tmp_ws_src_octomap_octomap_include_octomap_OccupancyOcTreeBase.h: Program Listing for File OccupancyOcTreeBase.h ============================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/octomap/octomap/include/octomap/OccupancyOcTreeBase.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* * 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 #include #include #include "octomap_types.h" #include "octomap_utils.h" #include "OcTreeBaseImpl.h" #include "AbstractOccupancyOcTree.h" namespace octomap { template class OccupancyOcTreeBase : public OcTreeBaseImpl { public: OccupancyOcTreeBase(double resolution); virtual ~OccupancyOcTreeBase(); OccupancyOcTreeBase(const OccupancyOcTreeBase& rhs); virtual void insertPointCloud(const Pointcloud& scan, const octomap::point3d& sensor_origin, double maxrange=-1., bool lazy_eval = false, bool discretize = false); virtual void insertPointCloud(const Pointcloud& scan, const point3d& sensor_origin, const pose6d& frame_origin, double maxrange=-1., bool lazy_eval = false, bool discretize = false); virtual void insertPointCloud(const ScanNode& scan, double maxrange=-1., bool lazy_eval = false, bool discretize = false); virtual void insertPointCloudRays(const Pointcloud& scan, const point3d& sensor_origin, double maxrange = -1., bool lazy_eval = false); virtual NODE* setNodeValue(const OcTreeKey& key, float log_odds_value, bool lazy_eval = false); virtual NODE* setNodeValue(const point3d& value, float log_odds_value, bool lazy_eval = false); virtual NODE* setNodeValue(double x, double y, double z, float log_odds_value, bool lazy_eval = false); virtual NODE* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false); virtual NODE* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false); virtual NODE* updateNode(double x, double y, double z, float log_odds_update, bool lazy_eval = false); virtual NODE* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false); virtual NODE* updateNode(const point3d& value, bool occupied, bool lazy_eval = false); virtual NODE* updateNode(double x, double y, double z, bool occupied, bool lazy_eval = false); virtual void toMaxLikelihood(); virtual bool insertRay(const point3d& origin, const point3d& end, double maxrange=-1.0, bool lazy_eval = false); virtual bool castRay(const point3d& origin, const point3d& direction, point3d& end, bool ignoreUnknownCells=false, double maxRange=-1.0) const; virtual bool getRayIntersection(const point3d& origin, const point3d& direction, const point3d& center, point3d& intersection, double delta=0.0) const; bool getNormals(const point3d& point, std::vector& normals, bool unknownStatus=true) const; //-- set BBX limit (limits tree updates to this bounding box) void useBBXLimit(bool enable) { use_bbx_limit = enable; } bool bbxSet() const { return use_bbx_limit; } void setBBXMin (const point3d& min); void setBBXMax (const point3d& max); point3d getBBXMin () const { return bbx_min; } point3d getBBXMax () const { return bbx_max; } point3d getBBXBounds () const; point3d getBBXCenter () const; bool inBBX(const point3d& p) const; bool inBBX(const OcTreeKey& key) const; //-- change detection on occupancy: void enableChangeDetection(bool enable) { use_change_detection = enable; } bool isChangeDetectionEnabled() const { return use_change_detection; } void resetChangeDetection() { changed_keys.clear(); } KeyBoolMap::const_iterator changedKeysBegin() const {return changed_keys.begin();} KeyBoolMap::const_iterator changedKeysEnd() const {return changed_keys.end();} size_t numChangesDetected() const { return changed_keys.size(); } void computeUpdate(const Pointcloud& scan, const octomap::point3d& origin, KeySet& free_cells, KeySet& occupied_cells, double maxrange); void computeDiscreteUpdate(const Pointcloud& scan, const octomap::point3d& origin, KeySet& free_cells, KeySet& occupied_cells, double maxrange); // -- I/O ----------------------------------------- std::istream& readBinaryData(std::istream &s); std::istream& readBinaryNode(std::istream &s, NODE* node); std::ostream& writeBinaryNode(std::ostream &s, const NODE* node) const; std::ostream& writeBinaryData(std::ostream &s) const; void updateInnerOccupancy(); virtual void integrateHit(NODE* occupancyNode) const; virtual void integrateMiss(NODE* occupancyNode) const; virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const; virtual void nodeToMaxLikelihood(NODE* occupancyNode) const; virtual void nodeToMaxLikelihood(NODE& occupancyNode) const; protected: OccupancyOcTreeBase(double resolution, unsigned int tree_depth, unsigned int tree_max_val); 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; point3d bbx_min; point3d bbx_max; OcTreeKey bbx_min_key; OcTreeKey bbx_max_key; bool use_change_detection; KeyBoolMap changed_keys; }; } // namespace #include "octomap/OccupancyOcTreeBase.hxx" #endif