VoxelGridStructure.h

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Geometry>
 
namespace Eigen
{
    using Matrix32f = Matrix<float, 3, 2>;
}
 
namespace visionx::voxelgrid
{
}
 
namespace visionx::voxelgrid
{
 
    /*
        /// A flat index, i.e. running index of a voxel in { 0, ..., number of voxels - 1 }.
        using FlatIndex = std::size_t;
        /// A grid index, i.e. index of voxel along each dimension (can be negative).
        using GridIndex = Eigen::Vector3i;
    */
 
    /**
     * @brief Geometric structure of a 3D voxel grid.
     *
     * The grid structure is defined by the voxel size(s) and grid size(s).
     * The voxel size (R^3) is the extent of a single voxel along each
     * axis (x, y, z). (Cubic voxels have constant extent along all axes).
     * The grid size (N^3) is the number of voxels along each axis.
     * (Cubic grids have constant number of voxels along all axes.)
     *
     * In addition, the voxel grid can have a pose, i.e. position and orientation.
     *
     * The total number of voxels to be contained in a structure is the
     * product of the grid size coefficients.
     *
     *
     * @par Grid Size and Voxel Index
     *
     * To indicate a voxel, one can use either of:
     * - a grid index i = (i_x i_y i_z) in Z^3  (Eigen::Vector3i)
     * - a flat index j in N^1                  (std::size_t)
     *
     * A grid index specifies the voxel index along each axis. A flat index
     * specifies a voxel's running index (from 0 to #voxels - 1).
     *
     * Each coefficient of a grid index is in the range
     *  `[ - gridSize/2, gridSize/2 )`
     * More precisely, it is in the set
     *  `{ - floor(gridSize/2), ..., 0, ..., ceil(gridSize/2) - 1 }`
     *
     * In other words,
     * - for an ODD  grid size: [ - floor(gridSize/2), floor(gridSize/2)    ]
     * - for an EVEN grid size: [ -       gridSize/2 ,       gridSize/2 - 1 ]
     *
     * For example (along one axis):
     *
     * | Grid Size |  Valid grid indices      |
     * +-----------+--------------------------+
     * |     3     |  { -1,  0,  1 }          |
     * |     4     |  { -2, -1,  0,  1 }      |
     * |     5     |  { -2, -1,  0,  1,  2 }  |
     *
     * Note that grid indices are centered around zero and can be negative.
     * A flat index is always non-negative.
     *
     *
     * @par Voxel Grid Origin
     *
     * The grid origin coincides with the center of voxel (0 0 0) and can be
     * chosen freely upon construction. It differs from the grid center in
     * axes with even grid size.
     *
     * @code
     *
     *  odd grid size (3)       even grid size (4)
     *   +---+---+---+          +---+---+---+---+       ^
     *   |   |   |   |  1       |   |   |   |   |  1    | voxel size
     *   +---+---+---+          +---+---+---+---+       v
     * c |   | o |   |  0       |   |   | o |   |  0
     *   +---+---+---+          +---+---+---+---+----- c
     *   |   |   |   | -1       |   |   |   |   | -1
     *   +---+---+---+          +---+---+---+---+
     *    -1   0   1            |   |   |   |   | -2
     *         c                +---+---+---+---+
     *                           -2  -1 | 0   1
     *                                  c
     * (i = origin, c = center)
     *
     * @endcode
     *
     */
    class VoxelGridStructure
    {
    public:
        /// Construct an empty grid structure (0 voxels with size 1.0).
        VoxelGridStructure();
 
        /// Construct a voxel grid structure with cubic voxels and cubic grid.
        VoxelGridStructure(int gridSize,
                           float voxelSize,
                           const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
                           const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
 
        /// Construct a voxel grid structure with cubic voxels.
        VoxelGridStructure(const Eigen::Vector3i& gridSize,
                           float voxelSize,
                           const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
                           const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
 
        /// Construct a voxel grid structure with cubic grid.
        VoxelGridStructure(int gridSize,
                           const Eigen::Vector3f& voxelSize,
                           const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
                           const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
 
        /// Construct a voxel grid structure.
        VoxelGridStructure(const Eigen::Vector3i& gridSize,
                           const Eigen::Vector3f& voxelSize,
                           const Eigen::Vector3f& origin = Eigen::Vector3f::Zero(),
                           const Eigen::Quaternionf& orientation = Eigen::Quaternionf::Identity());
 
 
        /// Get the grid size.
        Eigen::Vector3i getGridSize() const;
        /// Set the grid size for a cubic grid.
        void setGridSize(int gridSize);
        /// Set the grid size.
        void setGridSize(const Eigen::Vector3i& gridSize);
 
        /// Get the voxel size.
        Eigen::Vector3f getVoxelSize() const;
        /// Set the voxel size for cubic voxels.
        void setVoxelSize(float voxelSize);
        /// Set the voxel size.
        void setVoxelSize(const Eigen::Vector3f& voxelSize);
 
 
        /// Get the number of voxels contained in the structure.
        std::size_t getNumVoxels() const;
 
        /// Get the flat index for the given grid indices.
        std::size_t getFlatIndex(int x, int y, int z) const;
        /// Get the flat index for the given grid indices.
        std::size_t getFlatIndex(const Eigen::Vector3i& indices) const;
        /// Get the flat index into the grid data of the voxel closest to point.
        std::size_t getFlatIndex(const Eigen::Vector3f& point, bool local = false) const;
 
        /// Get the voxel indices of the voxel with the given index.
        Eigen::Vector3i getGridIndex(size_t index) const;
        /// Get the indices of the voxel closest to point.
        /// @param local If false (default), the point is transformed to local coordinate system.
        Eigen::Vector3i getGridIndex(const Eigen::Vector3f& point, bool local = false) const;
 
        /// Get the minimal (along each axis) grid index.
        Eigen::Vector3i getGridIndexMin() const;
        /// Get the maximal (along each axis) grid index.
        Eigen::Vector3i getGridIndexMax() const;
 
        /// Get the center of the voxel with the given indices.
        Eigen::Vector3f getVoxelCenter(std::size_t index, bool local = false) const;
        /// Get the center of the voxel with the given indices.
        Eigen::Vector3f getVoxelCenter(int x, int y, int z, bool local = false) const;
        /// Get the center of the voxel with the given indices.
        Eigen::Vector3f getVoxelCenter(const Eigen::Vector3i& indices, bool local = false) const;
 
 
        /// 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;
        /// 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);
 
        /// Get the grid orienation in the world frame.
        Eigen::Quaternionf getOrientation() const;
        /// Set the grid orienation in the world frame.
        void setOrientation(const Eigen::Quaternionf& 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;
        /**
         * @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 `setGridCenterPose()`.
         *
         * @see setPoseGridCenter()
         */
        void setCenter(const Eigen::Vector3f& center);
 
 
        /// 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;
        /// 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& pose);
 
        /// Get the grid pose positioned at the grid center in the world frame.
        Eigen::Matrix4f getCenterPose() const;
        /// 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& pose);
 
 
        /// Get extent of the grid along each axis (encompassing the whole voxels).
        Eigen::Vector3f getExtent() const;
        /// Get extent of the grid along each axis (encompassing only voxel centers).
        Eigen::Vector3f getExtentOfCenters() const;
        /// Get the local axis aligned bounding box of the grid (minimal/maximal values in columns).
        Eigen::Matrix32f getLocalBoundingBox() const;
        /// Get the local axis aligned bounding box of the voxel centers (minimal/maximal values in columns).
        Eigen::Matrix32f getLocalBoundingBoxOfCenters() const;
 
 
        /// Indicate whether the given point is inside the voxel.
        bool isInside(const Eigen::Vector3i& indices) const;
        /// Indicate whether the given point is inside the voxel.
        bool isInside(const Eigen::Vector3f& point, bool local = false) const;
 
        /// Assert that the given indices are valid grid indices.
        void checkIsInside(const Eigen::Vector3i& indices) const;
 
 
        /// Indicates whether `rhs` is equal to `*this`.
        /// Uses `v.isApprox()` to compare voxel size, origin and orientation.
        bool operator==(const VoxelGridStructure& rhs) const;
        bool operator!=(const VoxelGridStructure& rhs) const;
 
        /// Stream a human-readable description of the grid's structure.
        friend std::ostream& operator<<(std::ostream& os, const VoxelGridStructure& grid);
 
 
    private:
        /// Get half the grid size.
        Eigen::Vector3f halfGridSize() const;
        /// Get the translation from origin to grid center (in local or global frame).
        Eigen::Vector3f getOriginToGridCenter(bool local) const;
 
 
        /// The number of voxels in one direction.
        Eigen::Vector3i _gridSize = Eigen::Vector3i::Zero();
        /// The extent of a single voxel.
        Eigen::Vector3f _voxelSize = Eigen::Vector3f::Ones();
 
 
        /// The grid origin in the world frame. Coincides with the center of voxel (0 0 0).
        Eigen::Vector3f _origin = Eigen::Vector3f::Zero();
        /// The grid orientation in the world frame (pivots around origin).
        Eigen::Quaternionf _orientation = Eigen::Quaternionf::Identity();
 
 
    private:
        /// IO format for vectors.
        static const Eigen::IOFormat _iofVector;
        /// IO format for sizes (delimited by 'x').
        static const Eigen::IOFormat _iofTimes;
    };
 
 
} // namespace visionx::voxelgrid