VoxelGridStructure.cpp

Go to the documentation of this file.

#include "VoxelGridStructure.h"
 
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace visionx::voxelgrid
{
 
    template <typename Derived>
    using ScalarFunction = std::function<typename Eigen::MatrixBase<Derived>::Scalar(
        typename Eigen::MatrixBase<Derived>::Scalar)>;
 
    template <typename Derived>
    static Derived
    cwise(ScalarFunction<Derived> function, const Eigen::MatrixBase<Derived>& matrix)
    {
        Derived result = matrix;
        for (typename Derived::Index i = 0; i < result.size(); ++i)
        {
            result(i) = function(result(i));
        }
        return result;
    }
 
    const Eigen::IOFormat VoxelGridStructure::_iofVector = {5, 0, " ", " ", "", "", "[", "]"};
    const Eigen::IOFormat VoxelGridStructure::_iofTimes = {5, 0, "", " x ", "", "", "", ""};
 
    VoxelGridStructure::VoxelGridStructure()
    {
    }
 
    VoxelGridStructure::VoxelGridStructure(int gridSize,
                                           float voxelSize,
                                           const Eigen::Vector3f& origin,
                                           const Eigen::Quaternionf& orientation) :
        VoxelGridStructure(Eigen::Vector3i::Constant(gridSize),
                           Eigen::Vector3f::Constant(voxelSize),
                           origin,
                           orientation)
    {
    }
 
    VoxelGridStructure::VoxelGridStructure(const Eigen::Vector3i& gridSize,
                                           float voxelSize,
                                           const Eigen::Vector3f& origin,
                                           const Eigen::Quaternionf& orientation) :
        VoxelGridStructure(gridSize, Eigen::Vector3f::Constant(voxelSize), origin, orientation)
    {
    }
 
    VoxelGridStructure::VoxelGridStructure(int gridSize,
                                           const Eigen::Vector3f& voxelSize,
                                           const Eigen::Vector3f& origin,
                                           const Eigen::Quaternionf& orientation) :
        VoxelGridStructure(Eigen::Vector3i::Constant(gridSize), voxelSize, origin, orientation)
    {
    }
 
    VoxelGridStructure::VoxelGridStructure(const Eigen::Vector3i& gridSize,
                                           const Eigen::Vector3f& voxelSize,
                                           const Eigen::Vector3f& origin,
                                           const Eigen::Quaternionf& orientation) :
        _gridSize(gridSize), _voxelSize(voxelSize), _origin(origin), _orientation(orientation)
    {
        ARMARX_CHECK_NONNEGATIVE(voxelSize.minCoeff()) << "Voxel sizes must not be negative.";
        ARMARX_CHECK_NONNEGATIVE(gridSize.minCoeff()) << "Grid sizes must not be negative.";
    }
 
    Eigen::Vector3i
    VoxelGridStructure::getGridSize() const
    {
        return _gridSize;
    }
 
    void
    VoxelGridStructure::setGridSize(int gridSize)
    {
        setGridSize(Eigen::Vector3i::Constant(gridSize));
    }
 
    void
    VoxelGridStructure::setGridSize(const Eigen::Vector3i& gridSize)
    {
        _gridSize = gridSize;
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getVoxelSize() const
    {
        return _voxelSize;
    }
 
    void
    VoxelGridStructure::setVoxelSize(float voxelSize)
    {
        setVoxelSize(Eigen::Vector3f::Constant(voxelSize));
    }
 
    void
    VoxelGridStructure::setVoxelSize(const Eigen::Vector3f& voxelSize)
    {
        _voxelSize = voxelSize;
    }
 
    std::size_t
    VoxelGridStructure::getNumVoxels() const
    {
        return static_cast<std::size_t>(_gridSize.prod());
    }
 
    std::size_t
    VoxelGridStructure::getFlatIndex(int x, int y, int z) const
    {
        return getFlatIndex(Eigen::Vector3i(x, y, z));
    }
 
    std::size_t
    VoxelGridStructure::getFlatIndex(const Eigen::Vector3i& indices) const
    {
        const Eigen::Vector3f _halfGridSizes = halfGridSize();
 
        checkIsInside(indices);
 
        long index = static_cast<long>(
            (indices.x() + _halfGridSizes.x()) * _gridSize.z() * _gridSize.y() // skips y*z
            + (indices.y() + _halfGridSizes.y()) * _gridSize.z() // skips z
            + (indices.z() + _halfGridSizes.z()));
 
        ARMARX_CHECK_NONNEGATIVE(index);
        return static_cast<std::size_t>(index);
    }
 
    std::size_t
    VoxelGridStructure::getFlatIndex(const Eigen::Vector3f& point, bool local) const
    {
        return getFlatIndex(getGridIndex(point, local));
    }
 
    Eigen::Vector3i
    VoxelGridStructure::getGridIndex(size_t index) const
    {
        // FITS_SIZE triggers: comparison of unsigned expression >= 0 is always true [-Werror=type-limits]
        //ARMARX_CHECK_FITS_SIZE(index, getNumVoxels());
        ARMARX_CHECK_LESS(index, getNumVoxels());
        int x = static_cast<int>(index / static_cast<std::size_t>(_gridSize.z() * _gridSize.y()));
        int rest =
            static_cast<int>(index % static_cast<std::size_t>(_gridSize.z() * _gridSize.y()));
        int y = rest / _gridSize.z();
        int z = rest % _gridSize.z();
        return Eigen::Vector3i{x, y, z} + getGridIndexMin();
    }
 
    Eigen::Vector3i
    VoxelGridStructure::getGridIndex(const Eigen::Vector3f& point, bool local) const
    {
        Eigen::Vector3f pointLocal;
        if (local)
        {
            pointLocal = point;
        }
        else
        {
            // pose = local -> global   =>   inv_pose = global -> locals
            pointLocal =
                math::Helpers::TransformPosition(math::Helpers::InvertedPose(getPose()), point);
        }
 
        Eigen::Vector3f scaled = (pointLocal.array() / _voxelSize.array()).matrix();
 
        return cwise<Eigen::Vector3f>(&std::roundf, scaled).template cast<int>();
    }
 
    Eigen::Vector3i
    VoxelGridStructure::getGridIndexMin() const
    {
        // 3 -> 1.5 -> 1 -> -1
        // 4 -> 2.0 -> 2 -> -2
        // 5 -> 2.5 -> 2 -> -2
        // 6 -> 3.0 -> 3 -> -3
        return -cwise<Eigen::Vector3f>(&floorf, halfGridSize()).template cast<int>();
    }
 
    Eigen::Vector3i
    VoxelGridStructure::getGridIndexMax() const
    {
        // 3 -> 1.5 -> 1.0 -> 1
        // 4 -> 2.0 -> 1.5 -> 1
        // 5 -> 2.5 -> 2.0 -> 2
        // 6 -> 3.0 -> 2.5 -> 2
        const Eigen::Vector3f half = halfGridSize().array() - 0.5f;
        return cwise<Eigen::Vector3f>(&floorf, half).template cast<int>();
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getVoxelCenter(std::size_t index, bool local) const
    {
        return getVoxelCenter(getGridIndex(index), local);
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getVoxelCenter(int x, int y, int z, bool local) const
    {
        return getVoxelCenter({x, y, z}, local);
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getVoxelCenter(const Eigen::Vector3i& indices, bool local) const
    {
        checkIsInside(indices);
        auto center = _voxelSize.array() * indices.array().cast<float>();
        if (local)
        {
            return center;
        }
        else
        {
            return math::Helpers::TransformPosition(getPose(), center);
        }
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getOrigin() const
    {
        return _origin;
    }
 
    void
    VoxelGridStructure::setOrigin(const Eigen::Vector3f& origin)
    {
        _origin = origin;
    }
 
    Eigen::Quaternionf
    VoxelGridStructure::getOrientation() const
    {
        return _orientation;
    }
 
    void
    VoxelGridStructure::setOrientation(const Eigen::Quaternionf& orientation)
    {
        _orientation = orientation;
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getCenter() const
    {
        return _origin + getOriginToGridCenter(false);
    }
 
    void
    VoxelGridStructure::setCenter(const Eigen::Vector3f& center)
    {
        setOrigin(center - getOriginToGridCenter(false));
    }
 
    Eigen::Matrix4f
    VoxelGridStructure::getPose() const
    {
        return math::Helpers::Pose(_origin, _orientation);
    }
 
    void
    VoxelGridStructure::setPose(const Eigen::Matrix4f& pose)
    {
        setOrigin(math::Helpers::Position(pose));
        setOrientation(Eigen::Quaternionf(math::Helpers::Orientation(pose)));
    }
 
    Eigen::Matrix4f
    VoxelGridStructure::getCenterPose() const
    {
        return math::Helpers::Pose(getCenter(), _orientation);
    }
 
    void
    VoxelGridStructure::setCenterPose(const Eigen::Matrix4f& pose)
    {
        setOrientation(Eigen::Quaternionf(math::Helpers::Orientation(pose)));
        setCenter(math::Helpers::Position(pose));
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getExtent() const
    {
        return getExtentOfCenters() + _voxelSize;
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getExtentOfCenters() const
    {
        return _gridSize.cast<float>().array() * _voxelSize.array();
    }
 
    Eigen::Matrix32f
    VoxelGridStructure::getLocalBoundingBox() const
    {
        Eigen::Matrix32f bb = getLocalBoundingBoxOfCenters();
        bb.col(0) -= _voxelSize / 2;
        bb.col(1) += _voxelSize / 2;
        return bb;
    }
 
    Eigen::Matrix32f
    VoxelGridStructure::getLocalBoundingBoxOfCenters() const
    {
        Eigen::Matrix32f bb;
        bb.col(0) = getVoxelCenter(getGridIndexMin(), true);
        bb.col(1) = getVoxelCenter(getGridIndexMax(), true);
        return bb;
    }
 
    bool
    VoxelGridStructure::isInside(const Eigen::Vector3i& indices) const
    {
        const Eigen::Vector3i min = getGridIndexMin();
        const Eigen::Vector3i max = getGridIndexMax();
 
        for (int i = 0; i < indices.size(); ++i)
        {
            if (!(min(i) <= indices(i) && indices(i) <= max(i)))
            {
                return false;
            }
        }
        return true;
    }
 
    bool
    VoxelGridStructure::isInside(const Eigen::Vector3f& point, bool local) const
    {
        return isInside(getGridIndex(point, local));
    }
 
    void
    VoxelGridStructure::checkIsInside(const Eigen::Vector3i& indices) const
    {
        ARMARX_CHECK_EXPRESSION(isInside(indices))
            << "Indices must be inside voxel grid. "
            << "\n- given: " << indices.format(_iofVector)
            << "\n- allowed range: " << getGridIndexMin().format(_iofVector) << " .. "
            << getGridIndexMax().format(_iofVector);
    }
 
    bool
    VoxelGridStructure::operator==(const VoxelGridStructure& rhs) const
    {
        return _gridSize == rhs._gridSize && _voxelSize.isApprox(rhs._voxelSize) &&
               _origin.isApprox(rhs._origin) && _orientation.isApprox(rhs._orientation);
    }
 
    bool
    VoxelGridStructure::operator!=(const VoxelGridStructure& rhs) const
    {
        return !(*this == rhs);
    }
 
    Eigen::Vector3f
    VoxelGridStructure::halfGridSize() const
    {
        return _gridSize.cast<float>() / 2;
    }
 
    Eigen::Vector3f
    VoxelGridStructure::getOriginToGridCenter(bool local) const
    {
        Eigen::Vector3f shift = shift.Zero();
        for (Eigen::Vector3f::Index i = 0; i < shift.size(); ++i)
        {
            if (_gridSize(i) % 2 == 0)
            {
                shift(i) = -_voxelSize(i) / 2;
            }
        }
        return local ? shift : (_orientation * shift);
    }
 
    std::ostream&
    operator<<(std::ostream& os, const VoxelGridStructure& rhs)
    {
        os << "Structure with " << rhs.getNumVoxels() << " voxels";
        os << "\n- Origin:     \t" << rhs._origin.format(rhs._iofVector);
        os << "\n- Voxel size:\t" << rhs._voxelSize.format(rhs._iofTimes);
        os << "\n- Grid size: \t" << rhs._gridSize.format(rhs._iofTimes);
        os << "\n- Indices:     \t" << rhs.getGridIndexMin().format(rhs._iofVector) << " .. "
           << rhs.getGridIndexMax().format(rhs._iofVector);
        os << "\n- Centers:     \t"
           << rhs.getVoxelCenter(rhs.getGridIndexMin()).format(rhs._iofVector) << " .. "
           << rhs.getVoxelCenter(rhs.getGridIndexMax()).format(rhs._iofVector);
 
        return os;
    }
 
} // namespace visionx::voxelgrid