OccupancyOcTreeBase.h

Go to the documentation of this file.

/*
 * 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