OccupancyOcTreeBase.h

Go to the documentation of this file.

/*
 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
 * http://octomap.github.com/
 *
 * 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);

    OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
                   double maxrange=-1., bool pruning=true, bool lazy_eval = false))
    {
      this->insertPointCloud(scan, sensor_origin, maxrange, lazy_eval);
    }

    OCTOMAP_DEPRECATED(virtual void insertScan(const Pointcloud& scan, const point3d& sensor_origin,
                    const pose6d& frame_origin, double maxrange=-1., bool pruning = true, bool lazy_eval = false))
    {
      this->insertPointCloud(scan, sensor_origin, frame_origin, maxrange, lazy_eval);
    }

     OCTOMAP_DEPRECATED(virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false)){
       this->insertPointCloud(scan, maxrange, lazy_eval);
     }

     OCTOMAP_DEPRECATED( virtual void insertScanNaive(const Pointcloud& scan, const point3d& sensor_origin, double maxrange, bool lazy_eval = false)){
       this->insertPointCloudRays(scan, sensor_origin, maxrange, lazy_eval);
     }

     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 (point3d& min);
    void setBBXMax (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) const;

    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