VoxelGrid.hpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
 * Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Rainer Kartmann (rainer dot kartmann at student dot kit dot edu)
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <vector>
 
#include <Eigen/Geometry>
 
#include "VoxelGridStructure.h"
#include "exceptions.h"
 
namespace visionx::voxelgrid
{
 
    /**
     * @brief A 3D grid of voxels of type _VoxelT.
     *
     * A `VoxelGrid` three-dimensional regular grid of cuboids, called voxels.
     * Its geometric structure is defined by a `VoxelGridStructure`.
     * In addition, it stores voxel data of type _VoxelT (one per voxel).
     *
     * This is a generic implementation made to hold hold arbitrary data.
     * As such, there is no concept of "free/occupied" voxels".
     *
     * @see VoxelGridStructure
     */
    template <typename _VoxelT>
    class VoxelGrid
    {
    public:
        /// The voxel type.
        using VoxelT = _VoxelT;
 
 
    public:
        /// Construct a voxel grid with 0 voxels of size 1.0.
        VoxelGrid();
 
        /// Construct a voxel grid with the given structure and voxels with given value.
        VoxelGrid(const VoxelGridStructure& structure, const VoxelT& value = {});
 
        /**
         * @brief Construct a voxel grid with the given structure and data.
         * The size of `voxelData` must match the grid size of `structure`.
         * @throw error::InvalidVoxelDataSize If the size does not match.
         */
        VoxelGrid(const VoxelGridStructure& structure,
                  const std::vector<VoxelGrid::VoxelT>& voxelData);
 
        /// Construct a voxel grid with the same structure as other.
        template <typename OtherVoxelT>
        VoxelGrid(const VoxelGrid<OtherVoxelT>& other);
 
        /// Ordinary copy constructor for voxel grid with same VoxelT.
        VoxelGrid(const VoxelGrid<VoxelT>& other) = default;
 
        /// Reset the voxel data by `numVoxels()` voxels with given value.
        void
        reset(const VoxelT& value = {})
        {
            _voxels.assign(numVoxels(), value);
        }
 
        /// Get the voxel grid structure.
        VoxelGridStructure
        getStructure() const
        {
            return _struct;
        }
 
        /// Set the voxel grid structure and reset voxel data.
        void
        resetStructure(const VoxelGridStructure& structure)
        {
            this->_struct = structure;
            reset();
        }
 
        /// Get the voxel with the given index. @see `operator[]`
        VoxelT&
        getVoxel(std::size_t index)
        {
            return _voxels.at(index);
        }
 
        const VoxelT&
        getVoxel(std::size_t index) const
        {
            return _voxels.at(index);
        }
 
        /// Get the voxel with the given grid index. @see `operator[]`
        VoxelT&
        getVoxel(int x, int y, int z)
        {
            return getVoxel(getVoxelFlatIndex(x, y, z));
        }
 
        const VoxelT&
        getVoxel(int x, int y, int z) const
        {
            return getVoxel(getVoxelFlatIndex(x, y, z));
        }
 
        /// Get the voxel with the given grid index. @see `operator[]`
        VoxelT&
        getVoxel(const Eigen::Vector3i& index)
        {
            return getVoxel(getVoxelFlatIndex(index));
        }
 
        const VoxelT&
        getVoxel(const Eigen::Vector3i& index) const
        {
            return getVoxel(getVoxelFlatIndex(index));
        }
 
        /// Get the voxel closest to the given point.
        /// @param local If false (default), the point is transformed to local coordinate system.
        VoxelT&
        getVoxel(const Eigen::Vector3f& point, bool local = false)
        {
            return getVoxel(getVoxelGridIndex(point, local));
        }
 
        const VoxelT&
        getVoxel(const Eigen::Vector3f& point, bool local = false) const
        {
            return getVoxel(getVoxelGridIndex(point, local));
        }
 
        /// Get the voxels.
        /// @note `VoxelGrid` provides begin()/end() pairs, so it can be iterated over directly.
        const std::vector<VoxelT>&
        getVoxels() const
        {
            return _voxels;
        }
 
        /// Set the voxels. (Size must match `numVoxels()`).
        /// @throw error::InvalidVoxelDataSize If the size does not match.
        void
        setVoxels(const std::vector<VoxelT>& voxels)
        {
            if (voxels.size() != numVoxels())
            {
                throw error::InvalidVoxelDataSize(voxels.size(), numVoxels());
            }
            this->_voxels = voxels;
        }
 
        /// Get the number of voxels in the grid.
        std::size_t
        numVoxels() const
        {
            return getNumVoxels();
        }
 
        /// Get the number of voxels in the grid.
        std::size_t
        getNumVoxels() const
        {
            return _struct.getNumVoxels();
        }
 
        /// Get the grid size.
        Eigen::Vector3i
        getGridSizes() const
        {
            return _struct.getGridSize();
        }
 
        /// Set the grid size for a cubic grid. Resets the grid data.
        void
        setGridSizes(float gridSizes)
        {
            _struct.setGridSize(gridSizes);
        }
 
        /// Set the grid size. This resets the grid data.
        void
        setGridSizes(const Eigen::Vector3i& gridSizes)
        {
            _struct.setGridSize(gridSizes);
            reset();
        }
 
        /// Get the voxel size.
        Eigen::Vector3f
        getVoxelSizes() const
        {
            return _struct.getVoxelSize();
        }
 
        /// Set the voxel size of cubic voxels. The grid data is not updated.
        void
        setVoxelSizes(float voxelSize)
        {
            _struct.setVoxelSize(voxelSize);
        }
 
        /// Set the voxel size. The grid data is not updated.
        void
        setVoxelSizes(const Eigen::Vector3f& voxelSizes)
        {
            _struct.setVoxelSize(voxelSizes);
        }
 
        /// Get the flat index of the voxel with given grid index.
        std::size_t
        getVoxelFlatIndex(int x, int y, int z) const
        {
            return _struct.getFlatIndex(x, y, z);
        }
 
        /// Get the flat index of the voxel with given grid index.
        std::size_t
        getVoxelFlatIndex(const Eigen::Vector3i& index) const
        {
            return _struct.getFlatIndex(index);
        }
 
        /// Get the flat index of the voxel closest to `point`.
        std::size_t
        getVoxelFlatIndex(const Eigen::Vector3f& point, bool local = false) const
        {
            return _struct.getFlatIndex(point, local);
        }
 
        /// Get the grid index of the voxel with the given flat index.
        Eigen::Vector3i
        getVoxelGridIndex(size_t index) const
        {
            return _struct.getGridIndex(index);
        }
 
        /// Get the grid index of the voxel closest to point.
        /// @param local If false (default), the point is transformed to local coordinate system.
        Eigen::Vector3i
        getVoxelGridIndex(const Eigen::Vector3f& point, bool local = false) const
        {
            return _struct.getGridIndex(point, local);
        }
 
        /// Get the minimal (along each axis) voxel grid index.
        Eigen::Vector3i
        getVoxelGridIndexMin() const
        {
            return _struct.getGridIndexMin();
        }
 
        /// Get the maximal (along each axis) voxel grid index.
        Eigen::Vector3i
        getVoxelGridIndexMax() const
        {
            return _struct.getGridIndexMax();
        }
 
        /// Get the center of the voxel with the given index.
        Eigen::Vector3f
        getVoxelCenter(std::size_t index, bool local = false) const
        {
            return _struct.getVoxelCenter(index, local);
        }
 
        /// Get the center of the voxel with the given index.
        Eigen::Vector3f
        getVoxelCenter(int x, int y, int z, bool local = false) const
        {
            return _struct.getVoxelCenter(x, y, z, local);
        }
 
        /// Get the center of the voxel with the given index.
        Eigen::Vector3f
        getVoxelCenter(const Eigen::Vector3i& index, bool local = false) const
        {
            return _struct.getVoxelCenter(index, local);
        }
 
        /// Get the grid origin in the world frame (center of voxel [0 0 0]).
        /// Note that this differs from the geometric center for even grid sizes.
        /// @see getCenter()
        Eigen::Vector3f
        getOrigin() const
        {
            return _struct.getOrigin();
        }
 
        /// Set the grid origin the world frame (center of voxel [0 0 0]).
        /// Note that this differs from the geometric center for even grid sizes.
        /// @see setCenter()
        void
        setOrigin(const Eigen::Vector3f& value)
        {
            _struct.setOrigin(value);
        }
 
        /// Get the grid orienation in the world frame.
        Eigen::Quaternionf
        getOrientation() const
        {
            return _struct.getOrientation();
        }
 
        /// Set the grid orienation in the world frame.
        void
        setOrientation(const Eigen::Quaternionf& value)
        {
            _struct.setOrientation(value);
        }
 
        /**
         * @brief Get the geometric center of the grid the world frame
         * (which differs from the origin for even grid sizes).
         */
        Eigen::Vector3f
        getCenter() const
        {
            return _struct.getCenter();
        }
 
        /**
         * @brief Set the geometric center of the grid the world frame
         * (which differs from the origin for even grid sizes).
         *
         * @note This computes the origin from the given center using the
         * current orientation. If you want to set grid center and orientation,
         * set the orientation first, or (equivalently), use `setCenterPose()`.
         *
         * @see setPoseGridCenter()
         */
        void
        setCenter(const Eigen::Vector3f& value)
        {
            _struct.setCenter(value);
        }
 
        /// Get the grid pose in the world frame.
        /// Note that this differs from the geometric center for even grid sizes.
        /// @see getCenterPose()
        Eigen::Matrix4f
        getPose() const
        {
            return _struct.getPose();
        }
 
        /// Get the grid pose in the world frame.
        /// Note that this differs from the geometric center for even grid sizes.
        /// @see setCenterPose()
        void
        setPose(const Eigen::Matrix4f& value)
        {
            _struct.setPose(value);
        }
 
        /// Get the grid pose positioned at the grid center in the world frame.
        Eigen::Matrix4f
        getCenterPose() const
        {
            return _struct.getCenterPose();
        }
 
        /// Set the grid pose so that the grid center is the position of the
        /// given pose under the given orientation.
        void
        setCenterPose(const Eigen::Matrix4f& value)
        {
            _struct.setCenterPose(value);
        }
 
        /// Get extent of the grid along each axis (encompassing the whole voxels).
        Eigen::Vector3f
        getExtents() const
        {
            return _struct.getExtent();
        }
 
        /// Get extent of the grid along each axis (encompassing only voxel centers).
        Eigen::Vector3f
        getExtentsOfCenters() const
        {
            return _struct.getExtentOfCenters();
        }
 
        /// Get the local axis aligned bounding box of the grid (minimal/maximal values in columns).
        Eigen::Matrix32f
        getLocalBoundingBox() const
        {
            return _struct.getLocalBoundingBox();
        }
 
        /// Get the local axis aligned bounding box of the voxel centers (minimal/maximal values in columns).
        Eigen::Matrix32f
        getLocalBoundingBoxOfCenters() const
        {
            return _struct.getLocalBoundingBoxOfCenters();
        }
 
        /// Indicate whether the given point is inside the voxel.
        bool
        isInside(const Eigen::Vector3i& index) const
        {
            return _struct.isInside(index);
        }
 
        /// Indicate whether the given point is inside the voxel.
        bool
        isInside(const Eigen::Vector3f& point, bool local = false) const
        {
            return _struct.isInside(point, local);
        }
 
        /// Assert that the given index is a valid grid index.
        void
        checkIsInside(const Eigen::Vector3i& index) const
        {
            return _struct.checkIsInside(index);
        }
 
        /// Get the voxel with given flat index. @see `getVoxel(std::size_t)`
        VoxelT&
        operator[](std::size_t index)
        {
            return getVoxel(index);
        }
 
        const VoxelT&
        operator[](std::size_t index) const
        {
            return getVoxel(index);
        }
 
        /// Get the voxel with given grid index. @see `getVoxel(const Eigen::Vector3i&)`
        VoxelT&
        operator[](const Eigen::Vector3i& index)
        {
            return getVoxel(index);
        }
 
        const VoxelT&
        operator[](const Eigen::Vector3i& index) const
        {
            return getVoxel(index);
        }
 
        /// Get an iterator to the first voxel.
        typename std::vector<VoxelT>::iterator
        begin()
        {
            return _voxels.begin();
        }
 
        /// Get an iterator to the first voxel.
        typename std::vector<VoxelT>::const_iterator
        begin() const
        {
            return _voxels.begin();
        }
 
        /// Get an iterator to the first voxel.
        typename std::vector<VoxelT>::const_iterator
        cbegin() const
        {
            return _voxels.begin();
        }
 
        /// Get an iterator to the element following the last voxel.
        typename std::vector<VoxelT>::iterator
        end()
        {
            return _voxels.end();
        }
 
        /// Get an iterator to the element following the last voxel.
        typename std::vector<VoxelT>::const_iterator
        end() const
        {
            return _voxels.end();
        }
 
        /// Get an iterator to the element following the last voxel.
        typename std::vector<VoxelT>::const_iterator
        cend() const
        {
            return _voxels.end();
        }
 
 
        /// Stream a human-readable description of the grid's structure.
        template <class VoxelType>
        friend std::ostream& operator<<(std::ostream& os, const VoxelGrid<VoxelType>& grid);
 
 
    protected:
        /// The geometric structure.
        VoxelGridStructure _struct;
 
        /// The voxel data.
        std::vector<VoxelT> _voxels;
    };
 
    template <typename VT>
    VoxelGrid<VT>::VoxelGrid() : VoxelGrid({0, 1.f})
    {
    }
 
    template <typename VT>
    VoxelGrid<VT>::VoxelGrid(const VoxelGridStructure& structure, const VoxelT& value) :
        VoxelGrid(structure, std::vector<VoxelT>(structure.getNumVoxels(), value))
    {
    }
 
    template <typename VT>
    VoxelGrid<VT>::VoxelGrid(const VoxelGridStructure& structure,
                             const std::vector<VoxelGrid::VoxelT>& voxelData) :
        _struct(structure)
    {
        if (voxelData.size() != structure.getNumVoxels())
        {
            throw error::InvalidVoxelDataSize(voxelData.size(), structure.getNumVoxels());
        }
        _voxels = voxelData;
    }
 
    template <typename VT>
    template <typename OtherVoxelT>
    VoxelGrid<VT>::VoxelGrid(const VoxelGrid<OtherVoxelT>& other) : VoxelGrid(other.getStructure())
    {
    }
 
    template <typename VT>
    std::ostream&
    operator<<(std::ostream& os, const VoxelGrid<VT>& grid)
    {
        os << "VoxelGrid " << grid._struct;
        return os;
    }
 
} // namespace visionx::voxelgrid