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, Res ResT>
class Node;
 
 
 
template<typename DataT, int BlockSize, typename DerivedT>
class BlockSingleRes {
    public:
    typedef DataT DataType;
 
    static constexpr int min_scale = 0;
    static constexpr int current_scale = 0;
 
    BlockSingleRes(const DataType init_data = DataType());
 
    const DataType& data(const int voxel_idx) const;
 
    DataType& data(const int voxel_idx);
 
    const DataType& data(const Eigen::Vector3i& voxel_coord) const;
 
    DataType& data(const Eigen::Vector3i& voxel_coord);
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    private:
    std::array<DataType, BlockSize * BlockSize * BlockSize> block_data_;
 
    const DerivedT* underlying() const
    {
        return static_cast<const DerivedT*>(this);
    }
};
 
 
 
template<typename DataT, int BlockSize, typename DerivedT>
class BlockMultiRes {
};
 
 
 
template<Field FldT, Colour ColB, Id IdB, int BlockSize, typename DerivedT>
class BlockMultiRes<Data<FldT, ColB, IdB>, BlockSize, DerivedT> {
};
 
 
 
template<Colour ColB, Id IdB, int BlockSize, typename DerivedT>
class BlockMultiRes<Data<Field::TSDF, ColB, IdB>, BlockSize, DerivedT> {
    public:
    typedef Data<Field::TSDF, ColB, IdB> DataType;
 
    typedef DataType PastDataType;
 
    BlockMultiRes(const DataType init_data = DataType());
 
    struct DataUnion {
        const Eigen::Vector3i coord;
        const int scale;
        DataType& data;
        PastDataType& past_data;
        const int data_idx;
    };
 
    static constexpr int max_scale = math::log2_const(BlockSize);
    int min_scale = -1;
    int current_scale = -1;
 
    int getVoxelIdx(const Eigen::Vector3i& voxel_coord, const int scale) const;
 
 
    const DataType& data() const;
 
    DataType& data();
 
 
    const DataType& data(const int voxel_idx) const;
 
    DataType& data(const int voxel_idx);
 
    const DataType& data(const Eigen::Vector3i& voxel_coord) const;
 
    DataType& data(const Eigen::Vector3i& voxel_coord);
 
 
    const DataType&
    data(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out) const;
 
    DataType& data(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out);
 
 
    const DataType& data(const Eigen::Vector3i& voxel_coord, const int scale) const;
 
    DataType& data(const Eigen::Vector3i& voxel_coord, const int scale);
 
    DataUnion dataUnion(const Eigen::Vector3i& voxel_coord, const int scale);
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    private:
    static constexpr int compute_num_voxels()
    {
        size_t voxel_count = 0;
        for (int size_at_scale = BlockSize; size_at_scale > 0; size_at_scale /= 2) {
            voxel_count += math::cu(size_at_scale);
        }
        return voxel_count;
    }
 
    static constexpr std::array<int, max_scale + 1> compute_size_at_scales()
    {
        std::array<int, max_scale + 1> size_at_scales{};
        for (int size_at_scale = BlockSize, scale = 0; size_at_scale > 0;
             size_at_scale /= 2, scale++) {
            size_at_scales[scale] = size_at_scale;
        }
        return size_at_scales;
    }
 
    static constexpr std::array<int, max_scale + 1> compute_scale_offsets()
    {
        std::array<int, max_scale + 1> scale_offsets{0};
        for (int size_at_scale = BlockSize, scale = 1; size_at_scale > 1;
             size_at_scale /= 2, scale++) {
            scale_offsets[scale] = scale_offsets[scale - 1] + math::cu(size_at_scale);
        }
        return scale_offsets;
    }
 
    static constexpr int num_voxels_ = compute_num_voxels();
    static constexpr std::array<int, max_scale + 1> size_at_scales_ = compute_size_at_scales();
    static constexpr std::array<int, max_scale + 1> scale_offsets_ = compute_scale_offsets();
 
    std::array<DataType, num_voxels_> block_data_;
    std::array<PastDataType, num_voxels_> block_past_data_;
 
    const DerivedT* underlying() const
    {
        return static_cast<const DerivedT*>(this);
    }
};
 
 
 
// Forward decleration
template<typename DataT, Res ResT, int BlockSize>
class Block;
 
 
 
template<Colour ColB, Id IdB, int BlockSize, typename DerivedT>
class BlockMultiRes<Data<Field::Occupancy, ColB, IdB>, BlockSize, DerivedT> {
    public:
    typedef Data<Field::Occupancy, ColB, IdB> DataType;
 
    static constexpr int max_scale = math::log2_const(BlockSize);
    int min_scale = -1;
    int current_scale = max_scale;
    DataType init_data;
 
    BlockMultiRes(const DataType init_data = DataType());
 
    BlockMultiRes(const Block<Data<Field::Occupancy, ColB, IdB>, Res::Multi, BlockSize>& block);
 
    void operator=(const Block<Data<Field::Occupancy, ColB, IdB>, Res::Multi, BlockSize>& block);
 
    ~BlockMultiRes();
 
    int getVoxelIdx(const Eigen::Vector3i& voxel_coord, const int scale) const;
 
 
 
 
    const DataType& data(const Eigen::Vector3i& voxel_coord) const;
 
    DataType& data(const Eigen::Vector3i& voxel_coord);
 
 
    const DataType&
    data(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out) const;
 
    DataType& data(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out);
 
 
    const DataType& data(const Eigen::Vector3i& voxel_coord, const int scale) const;
 
    DataType& data(const Eigen::Vector3i& voxel_coord, const int scale);
 
    const DataType& minData(const Eigen::Vector3i& voxel_coord) const;
 
    DataType& minData(const Eigen::Vector3i& voxel_coord);
 
    const DataType&
    minData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out) const;
 
    DataType& minData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out);
 
    const DataType& minData(const Eigen::Vector3i& voxel_coord, const int scale) const;
 
    DataType& minData(const Eigen::Vector3i& voxel_coord, const int scale);
 
    const DataType& maxData(const Eigen::Vector3i& voxel_coord) const;
 
    DataType& maxData(const Eigen::Vector3i& voxel_coord);
 
    const DataType&
    maxData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out) const;
 
    DataType& maxData(const Eigen::Vector3i& voxel_coord, const int scale_in, int& scale_out);
 
    const DataType& maxData(const Eigen::Vector3i& voxel_coord, const int scale) const;
 
    DataType& maxData(const Eigen::Vector3i& voxel_coord, const int scale);
 
    void allocateDownTo(const int new_min_scale = 0);
 
    void deleteUpTo(const int new_min_scale);
 
    const DataType& data() const
    {
        assert(!block_data_.empty());
        assert(block_data_.front());
        return block_data_[0][0];
    }
 
    const DataType& minData() const
    {
        assert(!block_min_data_.empty());
        assert(block_min_data_.front());
        return block_min_data_[0][0];
    }
 
    const DataType& maxData() const
    {
        assert(!block_max_data_.empty());
        assert(block_max_data_.front());
        return block_max_data_[0][0];
    }
 
    const std::vector<DataType*>& blockData() const
    {
        return block_data_;
    }
 
    size_t currIntegrCount() const
    {
        return curr_integr_count_;
    }
 
    size_t currObservedCount() const
    {
        return curr_observed_count_;
    } 
 
    void incrCurrIntegrCount()
    {
        curr_integr_count_++;
    }
 
    void incrCurrObservedCount(bool do_increment = true);
 
    void resetCurrCount();
 
    void initCurrCout();
 
    int buffer_scale() const
    {
        return buffer_scale_;
    }
    size_t bufferIntegrCount() const
    {
        return buffer_integr_count_;
    }
    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();
 
    const DataType& bufferData(const Eigen::Vector3i& voxel_coord) const;
 
    DataType& bufferData(const Eigen::Vector3i& voxel_coord);
 
    const DataType& bufferData(const int voxel_idx) const
    {
        assert(buffer_data_);
        assert(voxel_idx >= 0);
        assert(voxel_idx < math::cu(BlockSize >> buffer_scale_));
        return buffer_data_[voxel_idx];
    }
 
    DataType& bufferData(const int voxel_idx)
    {
        assert(buffer_data_);
        assert(voxel_idx >= 0);
        assert(voxel_idx < math::cu(BlockSize >> buffer_scale_));
        return buffer_data_[voxel_idx];
    }
 
    const DataType& currData(const int voxel_idx) const
    {
        assert(curr_data_);
        assert(voxel_idx >= 0);
        assert(voxel_idx < math::cu(BlockSize >> current_scale));
        return curr_data_[voxel_idx];
    }
 
    DataType& currData(const int voxel_idx)
    {
        assert(curr_data_);
        assert(voxel_idx >= 0);
        assert(voxel_idx < math::cu(BlockSize >> current_scale));
        return curr_data_[voxel_idx];
    }
 
    DataType* blockDataAtScale(const int scale);
 
    DataType* blockMinDataAtScale(const int scale);
    DataType* blockMaxDataAtScale(const int scale);
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    private:
    std::vector<DataType*> block_data_;
    std::vector<DataType*> block_min_data_;
    std::vector<DataType*> block_max_data_;
 
    DataType* curr_data_ = nullptr; 
    size_t curr_integr_count_;      
    size_t curr_observed_count_;    
 
    DataType* buffer_data_ = nullptr; 
    int buffer_scale_ = -1;           
    size_t
        buffer_integr_count_; 
    size_t buffer_observed_count_; 
 
    const DerivedT* underlying() const
    {
        return static_cast<const DerivedT*>(this);
    }
};
 
 
 
template<typename DataT, Res ResT, int BlockSize>
class Block : public OctantBase,
              public std::conditional<
                  ResT == Res::Single,
                  BlockSingleRes<DataT, BlockSize, Block<DataT, ResT, BlockSize>>,
                  BlockMultiRes<DataT, BlockSize, Block<DataT, ResT, BlockSize>>>::type
 
{
    public:
    typedef DataT DataType;
 
    static constexpr int size = BlockSize;
    static constexpr int size_sq = math::sq(BlockSize);
    static constexpr int size_cu = math::cu(BlockSize);
 
    Block(Node<DataT, ResT>* parent_ptr, const int child_idx, const DataT init_data);
 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    static_assert(math::is_power_of_two(BlockSize));
};
 
} // namespace se
 
#include "impl/block_impl.hpp"
 
#endif // SE_BLOCK_HPP