include/octomap/AbstractOccupancyOcTree.h

Go to the documentation of this file.

#ifndef OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H
#define OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H

// $Id: AbstractOccupancyOcTree.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 "AbstractOcTree.h"
#include "octomap_utils.h"
#include "OcTreeNode.h"
#include "OcTreeKey.h"
#include <cassert>
#include <fstream>


namespace octomap {

  class AbstractOccupancyOcTree : public AbstractOcTree {
  public:
    AbstractOccupancyOcTree();
    virtual ~AbstractOccupancyOcTree() {};

    //-- IO

    bool writeBinary(const std::string& filename);

    bool writeBinary(std::ostream &s);

    bool writeBinaryConst(const std::string& filename) const;

    bool writeBinaryConst(std::ostream &s) const;

    virtual std::ostream& writeBinaryData(std::ostream &s) const = 0;
    
    bool readBinary(std::istream &s);
    
    bool readBinary(const std::string& filename);

    virtual std::istream& readBinaryData(std::istream &s) = 0;

    // -- occupancy queries

    inline bool isNodeOccupied(const OcTreeNode* occupancyNode) const{
      return (occupancyNode->getLogOdds() >= this->occ_prob_thres_log);
    }

    inline bool isNodeOccupied(const OcTreeNode& occupancyNode) const{
      return (occupancyNode.getLogOdds() >= this->occ_prob_thres_log);
    }

    inline bool isNodeAtThreshold(const OcTreeNode* occupancyNode) const{
      return (occupancyNode->getLogOdds() >= this->clamping_thres_max
          || occupancyNode->getLogOdds() <= this->clamping_thres_min);
    }

    inline bool isNodeAtThreshold(const OcTreeNode& occupancyNode) const{
      return (occupancyNode.getLogOdds() >= this->clamping_thres_max
          || occupancyNode.getLogOdds() <= this->clamping_thres_min);
    }

    // - update functions

    virtual OcTreeNode* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false) = 0;

    virtual OcTreeNode* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false) = 0;

    virtual OcTreeNode* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false) = 0;

    virtual OcTreeNode* updateNode(const point3d& value, bool occupied, bool lazy_eval = false) = 0;

    virtual void toMaxLikelihood() = 0;

    //-- parameters for occupancy and sensor model:

    void setOccupancyThres(double prob){occ_prob_thres_log = logodds(prob); }
    void setProbHit(double prob){prob_hit_log = logodds(prob); assert(prob_hit_log >= 0.0);}
    void setProbMiss(double prob){prob_miss_log = logodds(prob); assert(prob_miss_log <= 0.0);}
    void setClampingThresMin(double thresProb){clamping_thres_min = logodds(thresProb); }
    void setClampingThresMax(double thresProb){clamping_thres_max = logodds(thresProb); }

    double getOccupancyThres() const {return probability(occ_prob_thres_log); }
    float getOccupancyThresLog() const {return occ_prob_thres_log; }

    double getProbHit() const {return probability(prob_hit_log); }
    float getProbHitLog() const {return prob_hit_log; }
    double getProbMiss() const {return probability(prob_miss_log); }
    float getProbMissLog() const {return prob_miss_log; }

    double getClampingThresMin() const {return probability(clamping_thres_min); }
    float getClampingThresMinLog() const {return clamping_thres_min; }
    double getClampingThresMax() const {return probability(clamping_thres_max); }
    float getClampingThresMaxLog() const {return clamping_thres_max; }




  protected:
    bool readBinaryLegacyHeader(std::istream &s, unsigned int& size, double& res);
    
    // occupancy parameters of tree, stored in logodds:
    float clamping_thres_min;
    float clamping_thres_max;
    float prob_hit_log;
    float prob_miss_log;
    float occ_prob_thres_log;

    static const std::string binaryFileHeader;
  };

}; // end namespace


#endif