.. _program_listing_file_include_coal_octree.h:

Program Listing for File octree.h
=================================

|exhale_lsh| :ref:`Return to documentation for file <file_include_coal_octree.h>` (``include/coal/octree.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Software License Agreement (BSD License)
    *
    *  Copyright (c) 2011-2014, Willow Garage, Inc.
    *  Copyright (c) 2014-2015, Open Source Robotics Foundation
    *  Copyright (c) 2022-2024, Inria
    *  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 Open Source Robotics Foundation 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 COAL_OCTREE_H
   #define COAL_OCTREE_H
   
   #include <algorithm>
   
   #include <octomap/octomap.h>
   #include "coal/fwd.hh"
   #include "coal/BV/AABB.h"
   #include "coal/collision_object.h"
   
   namespace coal {
   
   class COAL_DLLAPI OcTree : public CollisionGeometry {
    protected:
     shared_ptr<const octomap::OcTree> tree;
   
     Scalar default_occupancy;
   
     Scalar occupancy_threshold;
     Scalar free_threshold;
   
    public:
     typedef octomap::OcTreeNode OcTreeNode;
   
     explicit OcTree(Scalar resolution)
         : tree(shared_ptr<const octomap::OcTree>(
               new octomap::OcTree(resolution))) {
       default_occupancy = Scalar(tree->getOccupancyThres());
   
       // default occupancy/free threshold is consistent with default setting from
       // octomap
       occupancy_threshold = Scalar(tree->getOccupancyThres());
       free_threshold = 0;
     }
   
     explicit OcTree(const shared_ptr<const octomap::OcTree>& tree_)
         : tree(tree_) {
       default_occupancy = Scalar(tree->getOccupancyThres());
   
       // default occupancy/free threshold is consistent with default setting from
       // octomap
       occupancy_threshold = Scalar(tree->getOccupancyThres());
       free_threshold = 0;
     }
   
     OcTree(const OcTree& other)
         : CollisionGeometry(other),
           tree(other.tree),
           default_occupancy(other.default_occupancy),
           occupancy_threshold(other.occupancy_threshold),
           free_threshold(other.free_threshold) {}
   
     OcTree* clone() const { return new OcTree(*this); }
   
     shared_ptr<const octomap::OcTree> getTree() const { return tree; }
   
     void exportAsObjFile(const std::string& filename) const;
   
     void computeLocalAABB() {
       typedef Eigen::Matrix<float, 3, 1> Vec3float;
       Vec3float max_extent, min_extent;
   
       octomap::OcTree::iterator it =
           tree->begin((unsigned char)tree->getTreeDepth());
       octomap::OcTree::iterator end = tree->end();
   
       if (it == end) return;
   
       {
         const octomap::point3d& coord =
             it.getCoordinate();  // getCoordinate returns a copy
         max_extent = min_extent = Eigen::Map<const Vec3float>(&coord.x());
         for (++it; it != end; ++it) {
           const octomap::point3d& coord = it.getCoordinate();
           const Vec3float pos = Eigen::Map<const Vec3float>(&coord.x());
           max_extent = max_extent.array().max(pos.array());
           min_extent = min_extent.array().min(pos.array());
         }
       }
   
       // Account for the size of the boxes.
       const Scalar resolution = Scalar(tree->getResolution());
       max_extent.array() += float(resolution / 2.);
       min_extent.array() -= float(resolution / 2.);
   
       aabb_local = AABB(min_extent.cast<Scalar>(), max_extent.cast<Scalar>());
       aabb_center = aabb_local.center();
       aabb_radius = (aabb_local.min_ - aabb_center).norm();
     }
   
     AABB getRootBV() const {
       Scalar delta =
           Scalar((1 << tree->getTreeDepth()) * tree->getResolution() / 2);
   
       // std::cout << "octree size " << delta << std::endl;
       return AABB(Vec3s(-delta, -delta, -delta), Vec3s(delta, delta, delta));
     }
   
     unsigned int getTreeDepth() const { return tree->getTreeDepth(); }
   
     unsigned long size() const { return tree->size(); }
   
     Scalar getResolution() const { return Scalar(tree->getResolution()); }
   
     OcTreeNode* getRoot() const { return tree->getRoot(); }
   
     bool isNodeOccupied(const OcTreeNode* node) const {
       // return tree->isNodeOccupied(node);
       return node->getOccupancy() >= occupancy_threshold;
     }
   
     bool isNodeFree(const OcTreeNode* node) const {
       // return false; // default no definitely free node
       return node->getOccupancy() <= free_threshold;
     }
   
     bool isNodeUncertain(const OcTreeNode* node) const {
       return (!isNodeOccupied(node)) && (!isNodeFree(node));
     }
   
     std::vector<Vec6s> toBoxes() const {
       std::vector<Vec6s> boxes;
       boxes.reserve(tree->size() / 2);
       for (octomap::OcTree::iterator
                it = tree->begin((unsigned char)tree->getTreeDepth()),
                end = tree->end();
            it != end; ++it) {
         // if(tree->isNodeOccupied(*it))
         if (isNodeOccupied(&*it)) {
           Scalar x = Scalar(it.getX());
           Scalar y = Scalar(it.getY());
           Scalar z = Scalar(it.getZ());
           Scalar size = Scalar(it.getSize());
           Scalar c = Scalar((*it).getOccupancy());
           Scalar t = Scalar(tree->getOccupancyThres());
   
           Vec6s box;
           box << x, y, z, size, c, t;
           boxes.push_back(box);
         }
       }
       return boxes;
     }
   
     std::vector<uint8_t> tobytes() const {
       typedef Eigen::Matrix<float, 3, 1> Vec3sloat;
       const size_t total_size = (tree->size() * sizeof(Scalar) * 3) / 2;
       std::vector<uint8_t> bytes;
       bytes.reserve(total_size);
   
       for (octomap::OcTree::iterator
                it = tree->begin((unsigned char)tree->getTreeDepth()),
                end = tree->end();
            it != end; ++it) {
         const Vec3s box_pos =
             Eigen::Map<Vec3sloat>(&it.getCoordinate().x()).cast<Scalar>();
         if (isNodeOccupied(&*it))
           std::copy(box_pos.data(), box_pos.data() + sizeof(Scalar) * 3,
                     std::back_inserter(bytes));
       }
   
       return bytes;
     }
   
     Scalar getOccupancyThres() const { return occupancy_threshold; }
   
     Scalar getFreeThres() const { return free_threshold; }
   
     Scalar getDefaultOccupancy() const { return default_occupancy; }
   
     void setCellDefaultOccupancy(Scalar d) { default_occupancy = d; }
   
     void setOccupancyThres(Scalar d) { occupancy_threshold = d; }
   
     void setFreeThres(Scalar d) { free_threshold = d; }
   
     OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) {
   #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
       return tree->getNodeChild(node, childIdx);
   #else
       return node->getChild(childIdx);
   #endif
     }
   
     const OcTreeNode* getNodeChild(const OcTreeNode* node,
                                    unsigned int childIdx) const {
   #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
       return tree->getNodeChild(node, childIdx);
   #else
       return node->getChild(childIdx);
   #endif
     }
   
     bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const {
   #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
       return tree->nodeChildExists(node, childIdx);
   #else
       return node->childExists(childIdx);
   #endif
     }
   
     bool nodeHasChildren(const OcTreeNode* node) const {
   #if OCTOMAP_VERSION_AT_LEAST(1, 8, 0)
       return tree->nodeHasChildren(node);
   #else
       return node->hasChildren();
   #endif
     }
   
     OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
   
     NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
   
    private:
     virtual bool isEqual(const CollisionGeometry& _other) const {
       const OcTree* other_ptr = dynamic_cast<const OcTree*>(&_other);
       if (other_ptr == nullptr) return false;
       const OcTree& other = *other_ptr;
   
       return (tree.get() == other.tree.get() || toBoxes() == other.toBoxes()) &&
              default_occupancy == other.default_occupancy &&
              occupancy_threshold == other.occupancy_threshold &&
              free_threshold == other.free_threshold;
     }
   
    public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   };
   
   static inline void computeChildBV(const AABB& root_bv, unsigned int i,
                                     AABB& child_bv) {
     const Scalar half = Scalar(0.5);
     if (i & 1) {
       child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * half;
       child_bv.max_[0] = root_bv.max_[0];
     } else {
       child_bv.min_[0] = root_bv.min_[0];
       child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * half;
     }
   
     if (i & 2) {
       child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * half;
       child_bv.max_[1] = root_bv.max_[1];
     } else {
       child_bv.min_[1] = root_bv.min_[1];
       child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * half;
     }
   
     if (i & 4) {
       child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * half;
       child_bv.max_[2] = root_bv.max_[2];
     } else {
       child_bv.min_[2] = root_bv.min_[2];
       child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * half;
     }
   }
   
   COAL_DLLAPI OcTreePtr_t
   makeOctree(const Eigen::Matrix<Scalar, Eigen::Dynamic, 3>& point_cloud,
              const Scalar resolution);
   
   }  // namespace coal
   
   #endif