Program Listing for File OccupancyOcTreeBase.h
↰ Return to documentation for file (/tmp/ws/src/octomap/octomap/include/octomap/OccupancyOcTreeBase.h)
/*
* 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 {
template <class NODE>
class OccupancyOcTreeBase : public OcTreeBaseImpl<NODE,AbstractOccupancyOcTree> {
public:
OccupancyOcTreeBase(double resolution);
virtual ~OccupancyOcTreeBase();
OccupancyOcTreeBase(const OccupancyOcTreeBase<NODE>& 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<point3d>& 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