include/octomap/OccupancyOcTreeBase.h

Go to the documentation of this file.

#ifndef OCTOMAP_OCCUPANCY_OCTREE_BASE_H
#define OCTOMAP_OCCUPANCY_OCTREE_BASE_H

// $Id: OccupancyOcTreeBase.h 420 2012-08-27 14:10:25Z ahornung $

/*
 * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
 * All rights reserved.
 *
 * 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.
 */

#include <list>
#include <stdlib.h>

#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 insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
                    double maxrange=-1., bool pruning=true, bool lazy_eval = false);

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

    virtual void insertScan(const ScanNode& scan, double maxrange=-1., bool pruning = true, bool lazy_eval = false);

    virtual void insertScanNaive(const Pointcloud& pc, const point3d& origin, double maxrange, bool pruning = true, 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(const OcTreeKey& key, bool occupied, bool lazy_eval = false);

    virtual NODE* updateNode(const point3d& value, 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;
   
    DEPRECATED( void getOccupied(point3d_list& node_centers, unsigned int max_depth = 0) const);
    
    DEPRECATED(void getOccupied(std::list<OcTreeVolume>& occupied_volumes, unsigned int max_depth = 0) const);

    DEPRECATED(void getOccupied(std::list<OcTreeVolume>& binary_nodes, std::list<OcTreeVolume>& delta_nodes,
                     unsigned int max_depth = 0) const);


    DEPRECATED(void getOccupiedLeafsBBX(point3d_list& node_centers, point3d min, point3d max) const);

    DEPRECATED(void getFreespace(std::list<OcTreeVolume>& free_volumes, unsigned int max_depth = 0) const);

    DEPRECATED(void getFreespace(std::list<OcTreeVolume>& binary_nodes, std::list<OcTreeVolume>& delta_nodes,
                      unsigned int max_depth = 0) 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; }
    void resetChangeDetection() { changed_keys.clear(); }

    KeyBoolMap::const_iterator changedKeysBegin() {return changed_keys.begin();}

    KeyBoolMap::const_iterator changedKeysEnd() {return changed_keys.end();}


    void computeUpdate(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 calcNumThresholdedNodes(unsigned int& num_thresholded, unsigned int& num_other) const;

    void updateInnerOccupancy();


    virtual void integrateHit(NODE* occupancyNode) const;
    virtual void integrateMiss(NODE* occupancyNode) const;
    // update logodds value of node, given update is added to current value.
    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);
    
    void updateInnerOccupancyRecurs(NODE* node, unsigned int depth);

    void getOccupiedLeafsBBXRecurs( point3d_list& node_centers, unsigned int max_depth, NODE* node, 
                                    unsigned int depth, const OcTreeKey& parent_key, 
                                    const OcTreeKey& min, const OcTreeKey& max) const;
    
    void toMaxLikelihoodRecurs(NODE* node, unsigned int depth, unsigned int max_depth);

    void calcNumThresholdedNodesRecurs (NODE* node,
                                        unsigned int& num_thresholded,
                                        unsigned int& num_other) const;

  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