block.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_BLOCK_HPP
#define SE_BLOCK_HPP

namespace se {



// Forward declare Node to avoid depending on the order of includes.
template<typename DataT, se::Res ResT>
class Node;



template<typename DerivedT, typename DataT, int BlockSize>
class BlockSingleRes {
    public:
    typedef DataT DataType;

    BlockSingleRes(const DataType init_data = DataType());

    inline const DataType& getData(const int voxel_idx) const;

    inline DataType& getData(const int voxel_idx);

    inline const DataType& getData(const Eigen::Vector3i& voxel_coord) const;

    inline DataType& getData(const Eigen::Vector3i& voxel_coord);

    inline void setData(const unsigned voxel_idx, const DataT& data);

    inline void setData(const Eigen::Vector3i& voxel_coord, const DataT& data);


    static inline int getMinScale()
    {
        return min_scale_;
    }

    static inline int getCurrentScale()
    {
        return curr_scale_;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
    std::array<DataType, BlockSize * BlockSize * BlockSize> block_data_;
    static constexpr int min_scale_ = 0;
    static constexpr int curr_scale_ = 0;

    DerivedT& underlying()
    {
        return static_cast<DerivedT&>(*this);
    }
    const DerivedT& underlying() const
    {
        return static_cast<const DerivedT&>(*this);
    }
};



template<typename DataT, int BlockSize, typename DerivedT>
class BlockMultiRes {
};



template<Field FldT, Colour ColB, Semantics SemB, int BlockSize, typename DerivedT>
class BlockMultiRes<se::Data<FldT, ColB, SemB>, BlockSize, DerivedT> {
};



template<Colour ColB, Semantics SemB, int BlockSize, typename DerivedT>
class BlockMultiRes<se::Data<se::Field::TSDF, ColB, SemB>, BlockSize, DerivedT> {
    public:
    typedef se::Data<se::Field::TSDF, ColB, SemB> DataType;
    typedef se::DeltaData<se::Field::TSDF, ColB, SemB> PropDataType;

    BlockMultiRes(const DataType init_data = DataType());

    struct DataUnion {
        DataUnion(){};
        Eigen::Vector3i coord;
        int scale;

        DataType data;
        PropDataType prop_data;

        int data_idx;
        int prop_data_idx;
    };


    inline int getVoxelIdx(const Eigen::Vector3i& voxel_coord) const;

    inline int getVoxelIdx(const Eigen::Vector3i& voxel_coord, const int scale) const;


    inline const DataType& getData() const;

    inline DataType& getData();


    inline const DataType& getData(const int voxel_idx) const;

    inline DataType& getData(const int voxel_idx);

    inline const DataType& getData(const Eigen::Vector3i& voxel_coord) const;

    inline DataType& getData(const Eigen::Vector3i& voxel_coord);


    inline const DataType&
    getData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out) const;

    inline DataType&
    getData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out);


    inline const DataType& getData(const Eigen::Vector3i& voxel_coord, const int scale) const;

    inline DataType& getData(const Eigen::Vector3i& voxel_coord, const int scale);


    inline const DataUnion getDataUnion(const Eigen::Vector3i& voxel_coord, const int scale) const;

    inline DataUnion getDataUnion(const Eigen::Vector3i& voxel_coord, const int scale);


    inline void setData(const int voxel_idx, const DataType& data);

    inline void setData(const Eigen::Vector3i& voxel_coord, const DataType& data);

    inline void setData(const Eigen::Vector3i& voxel_coord, const int scale, const DataType& data);

    inline void setDataUnion(const DataUnion& data_union);



    inline int getMinScale() const
    {
        return min_scale_;
    }

    inline void setMinScale(const int min_scale)
    {
        min_scale_ = min_scale;
    }

    static constexpr inline int getMaxScale()
    {
        return max_scale_;
    }

    inline int getCurrentScale() const
    {
        return curr_scale_;
    }

    inline void setCurrentScale(const int curr_scale)
    {
        curr_scale_ = curr_scale;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
    static constexpr int max_scale_ = math::log2_const(BlockSize);

    static constexpr int compute_num_voxels()
    {
        size_t voxel_count = 0;
        unsigned int size_at_scale = BlockSize;
        while (size_at_scale >= 1) {
            voxel_count += size_at_scale * size_at_scale * size_at_scale;

            size_at_scale = size_at_scale >> 1;
        }
        return voxel_count;
    }

    static constexpr std::array<int, se::math::log2_const(BlockSize) + 1> compute_size_at_scales()
    {
        std::array<int, se::math::log2_const(BlockSize) + 1> size_at_scales{};

        int size_at_scale = BlockSize;
        int scale = 0;
        while (size_at_scale >= 1) {
            size_at_scales[scale] = size_at_scale;
            size_at_scale = size_at_scale >> 1;
            scale++;
        }
        return size_at_scales;
    }

    static constexpr std::array<int, se::math::log2_const(BlockSize) + 1> compute_scale_offsets()
    {
        std::array<int, se::math::log2_const(BlockSize) + 1> scale_offsets{};

        unsigned int size_at_scale = BlockSize;
        scale_offsets[0] = 0;
        int scale = 1;
        while (size_at_scale > 1) {
            scale_offsets[scale] =
                scale_offsets[scale - 1] + size_at_scale * size_at_scale * size_at_scale;
            size_at_scale = size_at_scale >> 1;
            scale++;
        }
        return scale_offsets;
    }

    static constexpr int num_voxels_ = compute_num_voxels();

    static constexpr std::array<int, se::math::log2_const(BlockSize) + 1> size_at_scales_ =
        compute_size_at_scales();

    static constexpr std::array<int, se::math::log2_const(BlockSize) + 1> scale_offsets_ =
        compute_scale_offsets();


    std::array<DataType, num_voxels_> block_data_;
    std::array<PropDataType, num_voxels_> block_prop_data_;

    int min_scale_;
    int curr_scale_;

    DerivedT& underlying()
    {
        return static_cast<DerivedT&>(*this);
    }
    const DerivedT& underlying() const
    {
        return static_cast<const DerivedT&>(*this);
    }
};



// Forward decleration
template<typename DataT,
         Res ResT = se::Res::Single,
         int BlockSize = 8,
         typename PolicyT = std::enable_if_t<math::is_power_of_two(
             BlockSize)> 
         >
class Block;



template<Colour ColB, Semantics SemB, int BlockSize, typename DerivedT>
class BlockMultiRes<se::Data<se::Field::Occupancy, ColB, SemB>, BlockSize, DerivedT> {
    public:
    typedef se::Data<se::Field::Occupancy, ColB, SemB> DataType;
    typedef se::DeltaData<se::Field::Occupancy, ColB, SemB> PropDataType;

    BlockMultiRes(const DataType init_data = DataType());

    BlockMultiRes(
        const Block<se::Data<se::Field::Occupancy, ColB, SemB>, se::Res::Multi, BlockSize>& block);

    void operator=(
        const Block<se::Data<se::Field::Occupancy, ColB, SemB>, se::Res::Multi, BlockSize>& block);

    ~BlockMultiRes();


    inline const DataType& getInitData() const
    {
        return init_data_;
    };
    inline DataType& getInitData()
    {
        return init_data_;
    };

    inline void setInitData(const DataType& init_data)
    {
        init_data_ = init_data;
    };


    inline int getVoxelIdx(const Eigen::Vector3i& voxel_coord) const;

    inline int getVoxelIdx(const Eigen::Vector3i& voxel_coord, const int scale) const;




    inline const DataType& getData(const Eigen::Vector3i& voxel_coord) const;

    inline DataType& getData(const Eigen::Vector3i& voxel_coord);


    inline const DataType&
    getData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out) const;

    inline DataType&
    getData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out);


    inline const DataType& getData(const Eigen::Vector3i& voxel_coord, const int scale) const;

    inline DataType& getData(const Eigen::Vector3i& voxel_coord, const int scale);

    inline const DataType& getMaxData(const Eigen::Vector3i& voxel_coord) const;

    inline DataType& getMaxData(const Eigen::Vector3i& voxel_coord);

    inline const DataType&
    getMaxData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out) const;

    inline DataType&
    getMaxData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out);

    inline const DataType& getMaxData(const Eigen::Vector3i& voxel_coord, const int scale) const;

    inline DataType& getMaxData(const Eigen::Vector3i& voxel_coord, const int scale);

    inline const DataType& getMinData() const
    {
        return min_data_;
    };


    inline void setData(const Eigen::Vector3i& voxel_coord, const DataType& data);

    inline void setData(const Eigen::Vector3i& voxel_coord, const int scale, const DataType& data);

    inline void setMaxData(const Eigen::Vector3i& voxel_coord, const DataType& data);


    inline void
    setMaxData(const Eigen::Vector3i& voxel_coord, const int scale, const DataType& data);

    void setMinData(const DataType& min_data)
    {
        min_data_ = min_data;
    }

    void allocateDownTo();

    inline void allocateDownTo(const int new_min_scale);

    void deleteUpTo(const int new_min_scale);

    inline const DataType getData() const
    {
        return block_data_[0][0];
    }

    inline const DataType getMaxData() const
    {
        return block_max_data_[0][0];
    }

    se::field_t meanValue()
    {
        return get_field(getData());
    }

    se::field_t maxValue()
    {
        return get_field(getMaxData());
    }

    inline const std::vector<DataType*>& blockData() const
    {
        return block_data_;
    }

    inline std::vector<DataType*>& blockData()
    {
        return block_data_;
    }

    inline const std::vector<DataType*>& blockMaxData() const
    {
        return block_max_data_;
    }

    inline std::vector<DataType*>& blockMaxData()
    {
        return block_max_data_;
    }

    inline const size_t& currIntegrCount() const
    {
        return curr_integr_count_;
    }

    inline const size_t& currObservedCount() const
    {
        return curr_observed_count_;
    } 

    void incrCurrIntegrCount()
    {
        curr_integr_count_++;
    }

    void incrCurrObservedCount(bool do_increment = true);

    void resetCurrCount();

    void initCurrCout();

    const int& buffer_scale() const
    {
        return buffer_scale_;
    }
    const size_t& bufferIntegrCount() const
    {
        return buffer_integr_count_;
    }
    const size_t& bufferObservedCount() const
    {
        return buffer_observed_count_;
    }

    void incrBufferIntegrCount(const bool do_increment = true);


    void incrBufferObservedCount(const bool do_increment = true);
    void resetBufferCount();

    void resetBuffer();

    void initBuffer(const int buffer_scale);

    bool switchData();

    DataType& bufferData(const Eigen::Vector3i& voxel_coord) const;

    DataType& bufferData(const Eigen::Vector3i& voxel_coord);

    DataType& bufferData(const int voxel_idx) const
    {
        return buffer_data_[voxel_idx];
    }

    DataType& bufferData(const int voxel_idx)
    {
        return buffer_data_[voxel_idx];
    }

    DataType& currData(const int voxel_idx) const
    {
        return curr_data_[voxel_idx];
    }

    DataType& currData(const int voxel_idx)
    {
        return curr_data_[voxel_idx];
    }

    DataType* blockDataAtScale(const int scale);

    DataType* blockMaxDataAtScale(const int scale);


    inline int getMinScale() const
    {
        return min_scale_;
    }

    inline void setMinScale(const int min_scale)
    {
        min_scale_ = min_scale;
    }

    static constexpr inline int getMaxScale()
    {
        return max_scale_;
    }

    inline int getCurrentScale() const
    {
        return curr_scale_;
    }

    inline void setCurrentScale(const int curr_scale)
    {
        curr_scale_ = curr_scale;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
    static constexpr int max_scale_ = math::log2_const(BlockSize);

    void initialiseData(DataType* voxel_data, const int num_voxels)
    {
        std::fill(voxel_data, voxel_data + num_voxels, init_data_);
    }

    std::vector<DataType*> block_data_;
    std::vector<DataType*> block_max_data_;

    DataType* curr_data_ = nullptr; 
    size_t curr_integr_count_;      
    size_t curr_observed_count_;    

    int curr_scale_;
    int min_scale_;

    DataType* buffer_data_ = nullptr; 
    int buffer_scale_;                
    size_t
        buffer_integr_count_; 
    size_t buffer_observed_count_; 

    DataType min_data_;
    DataType init_data_;

    DerivedT& underlying()
    {
        return static_cast<DerivedT&>(*this);
    }
    const DerivedT& underlying() const
    {
        return static_cast<const DerivedT&>(*this);
    }
};


template<typename DataT, Res ResT, int BlockSize, typename PolicyT>
class Block
        : public OctantBase,
          public std::conditional<ResT == Res::Single,
                                  BlockSingleRes<Block<DataT, ResT, BlockSize>, DataT, BlockSize>,
                                  BlockMultiRes<DataT, BlockSize, Block<DataT, ResT, BlockSize>>>::
              type 

{
    public:
    typedef DataT DataType;
    static constexpr int size = BlockSize;
    static constexpr int size_qu = BlockSize * BlockSize;
    static constexpr int size_cu = BlockSize * BlockSize * BlockSize;

    Block(se::Node<DataT, ResT>* parent_ptr, const int child_idx, const DataT init_data);

    static constexpr unsigned int getSize()
    {
        return BlockSize;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};



} // namespace se



#include "impl/block_impl.hpp"



#endif // SE_BLOCK_HPP