octree.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2016-2019 Emanuele Vespa
 * SPDX-FileCopyrightText: 2019-2021 Smart Robotics Lab, Imperial College London, Technical University of Munich
 * SPDX-FileCopyrightText: 2019-2021 Nils Funk
 * SPDX-FileCopyrightText: 2019-2021 Sotiris Papatheodorou
 * SPDX-License-Identifier: BSD-3-Clause
 */

#ifndef SE_OCTREE_HPP
#define SE_OCTREE_HPP

#include <memory>

#include "se/map/octant/octant.hpp"
#include "se/map/octree/iterator.hpp"
#include "se/map/utils/key_util.hpp"
#include "se/map/utils/memory_pool.hpp"
#include "se/map/utils/setup_util.hpp"



namespace se {

template<typename DataT, Res ResT = Res::Single, int BlockSize = 8>
class Octree {
    public:
    typedef std::shared_ptr<Octree<DataT, ResT, BlockSize>> Ptr;

    typedef DataT DataType;
    typedef Node<DataT, ResT> NodeType;
    typedef Block<DataT, ResT, BlockSize> BlockType;

    typedef se::BoostMemoryPool<NodeType, BlockType> MemoryPool;

    // Compile-time constant expressions
    // # of voxels per side in a voxel block
    static constexpr unsigned int block_size = BlockSize;
    // The maximum scale of a block
    static constexpr se::scale_t max_block_scale = math::log2_const(BlockSize);

    Octree(const int size);

    ~Octree(){};                               
    Octree(const Octree&) = delete;            
    Octree& operator=(const Octree&) = delete; 

    OctreeIterator<Octree<DataT, ResT, BlockSize>> begin();
    OctreeIterator<Octree<DataT, ResT, BlockSize>> end();

    inline bool contains(const Eigen::Vector3i& voxel_coord) const;

    inline se::OctantBase* getRoot()
    {
        return root_ptr_;
    };

    inline se::OctantBase* getRoot() const
    {
        return root_ptr_;
    };

    inline int getSize() const
    {
        return size_;
    }

    inline int getMaxScale() const
    {
        return se::math::log2_const(size_);
    }

    inline int getBlockDepth() const
    {
        return se::math::log2_const(size_) - se::math::log2_const(BlockSize);
    }

    inline bool allocate(NodeType* parent_ptr,
                         const int child_idx,
                         se::OctantBase*& child_ptr); 

    inline se::OctantBase* allocate(NodeType* parent_ptr, const int child_idx);

    inline bool allocateAll(NodeType* parent_ptr,
                            const int child_idx,
                            se::OctantBase*& child_ptr); 

    inline se::OctantBase* allocateAll(NodeType* parent_ptr, const int child_idx);

    inline void deleteChildren(NodeType* parent_ptr);

    static constexpr se::Field fld_ = DataT::fld_;
    static constexpr se::Colour col_ = DataT::col_;
    static constexpr se::Semantics sem_ = DataT::sem_;

    static constexpr se::Res res_ = ResT;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
    int size_;                           
    se::OctantBase* root_ptr_ = nullptr; 

    MemoryPool memory_pool_; 
};



template<typename DataT, Res ResT, int BlockSize>
constexpr se::scale_t Octree<DataT, ResT, BlockSize>::max_block_scale;

} // namespace se

#include "impl/octree_impl.hpp"

#endif // SE_OCTREE_HPP