|
A 3D grid of voxels of type _VoxelT. More...
#include <VisionX/libraries/VoxelGridCore/VoxelGrid.hpp>
Public Types | |
using | VoxelT = _VoxelT |
The voxel type. More... | |
Public Member Functions | |
std::vector< VoxelT >::iterator | begin () |
Get an iterator to the first voxel. More... | |
std::vector< VoxelT >::const_iterator | begin () const |
Get an iterator to the first voxel. More... | |
std::vector< VoxelT >::const_iterator | cbegin () const |
Get an iterator to the first voxel. More... | |
std::vector< VoxelT >::const_iterator | cend () const |
Get an iterator to the element following the last voxel. More... | |
void | checkIsInside (const Eigen::Vector3i &index) const |
Assert that the given index is a valid grid index. More... | |
std::vector< VoxelT >::iterator | end () |
Get an iterator to the element following the last voxel. More... | |
std::vector< VoxelT >::const_iterator | end () const |
Get an iterator to the element following the last voxel. More... | |
Eigen::Vector3f | getCenter () const |
Get the geometric center of the grid the world frame (which differs from the origin for even grid sizes). More... | |
Eigen::Matrix4f | getCenterPose () const |
Get the grid pose positioned at the grid center in the world frame. More... | |
Eigen::Vector3f | getExtents () const |
Get extent of the grid along each axis (encompassing the whole voxels). More... | |
Eigen::Vector3f | getExtentsOfCenters () const |
Get extent of the grid along each axis (encompassing only voxel centers). More... | |
Eigen::Vector3i | getGridSizes () const |
Get the grid size. More... | |
Eigen::Matrix32f | getLocalBoundingBox () const |
Get the local axis aligned bounding box of the grid (minimal/maximal values in columns). More... | |
Eigen::Matrix32f | getLocalBoundingBoxOfCenters () const |
Get the local axis aligned bounding box of the voxel centers (minimal/maximal values in columns). More... | |
std::size_t | getNumVoxels () const |
Get the number of voxels in the grid. More... | |
Eigen::Quaternionf | getOrientation () const |
Get the grid orienation in the world frame. More... | |
Eigen::Vector3f | getOrigin () const |
Get the grid origin in the world frame (center of voxel [0 0 0]). More... | |
Eigen::Matrix4f | getPose () const |
Get the grid pose in the world frame. More... | |
VoxelGridStructure | getStructure () const |
Get the voxel grid structure. More... | |
VoxelT & | getVoxel (const Eigen::Vector3f &point, bool local=false) |
Get the voxel closest to the given point. More... | |
const VoxelT & | getVoxel (const Eigen::Vector3f &point, bool local=false) const |
VoxelT & | getVoxel (const Eigen::Vector3i &index) |
Get the voxel with the given grid index. More... | |
const VoxelT & | getVoxel (const Eigen::Vector3i &index) const |
VoxelT & | getVoxel (int x, int y, int z) |
Get the voxel with the given grid index. More... | |
const VoxelT & | getVoxel (int x, int y, int z) const |
VoxelT & | getVoxel (std::size_t index) |
Get the voxel with the given index. More... | |
const VoxelT & | getVoxel (std::size_t index) const |
Eigen::Vector3f | getVoxelCenter (const Eigen::Vector3i &index, bool local=false) const |
Get the center of the voxel with the given index. More... | |
Eigen::Vector3f | getVoxelCenter (int x, int y, int z, bool local=false) const |
Get the center of the voxel with the given index. More... | |
Eigen::Vector3f | getVoxelCenter (std::size_t index, bool local=false) const |
Get the center of the voxel with the given index. More... | |
std::size_t | getVoxelFlatIndex (const Eigen::Vector3f &point, bool local=false) const |
Get the flat index of the voxel closest to point . More... | |
std::size_t | getVoxelFlatIndex (const Eigen::Vector3i &index) const |
Get the flat index of the voxel with given grid index. More... | |
std::size_t | getVoxelFlatIndex (int x, int y, int z) const |
Get the flat index of the voxel with given grid index. More... | |
Eigen::Vector3i | getVoxelGridIndex (const Eigen::Vector3f &point, bool local=false) const |
Get the grid index of the voxel closest to point. More... | |
Eigen::Vector3i | getVoxelGridIndex (size_t index) const |
Get the grid index of the voxel with the given flat index. More... | |
Eigen::Vector3i | getVoxelGridIndexMax () const |
Get the maximal (along each axis) voxel grid index. More... | |
Eigen::Vector3i | getVoxelGridIndexMin () const |
Get the minimal (along each axis) voxel grid index. More... | |
const std::vector< VoxelT > & | getVoxels () const |
Get the voxels. More... | |
Eigen::Vector3f | getVoxelSizes () const |
Get the voxel size. More... | |
bool | isInside (const Eigen::Vector3f &point, bool local=false) const |
Indicate whether the given point is inside the voxel. More... | |
bool | isInside (const Eigen::Vector3i &index) const |
Indicate whether the given point is inside the voxel. More... | |
std::size_t | numVoxels () const |
Get the number of voxels in the grid. More... | |
VoxelT & | operator[] (const Eigen::Vector3i &index) |
Get the voxel with given grid index. More... | |
const VoxelT & | operator[] (const Eigen::Vector3i &index) const |
VoxelT & | operator[] (std::size_t index) |
Get the voxel with given flat index. More... | |
const VoxelT & | operator[] (std::size_t index) const |
void | reset (const VoxelT &value={}) |
Reset the voxel data by numVoxels() voxels with given value. More... | |
void | resetStructure (const VoxelGridStructure &structure) |
Set the voxel grid structure and reset voxel data. More... | |
void | setCenter (const Eigen::Vector3f &value) |
Set the geometric center of the grid the world frame (which differs from the origin for even grid sizes). More... | |
void | setCenterPose (const Eigen::Matrix4f &value) |
Set the grid pose so that the grid center is the position of the given pose under the given orientation. More... | |
void | setGridSizes (const Eigen::Vector3i &gridSizes) |
Set the grid size. This resets the grid data. More... | |
void | setGridSizes (float gridSizes) |
Set the grid size for a cubic grid. Resets the grid data. More... | |
void | setOrientation (const Eigen::Quaternionf &value) |
Set the grid orienation in the world frame. More... | |
void | setOrigin (const Eigen::Vector3f &value) |
Set the grid origin the world frame (center of voxel [0 0 0]). More... | |
void | setPose (const Eigen::Matrix4f &value) |
Get the grid pose in the world frame. More... | |
void | setVoxels (const std::vector< VoxelT > &voxels) |
Set the voxels. More... | |
void | setVoxelSizes (const Eigen::Vector3f &voxelSizes) |
Set the voxel size. The grid data is not updated. More... | |
void | setVoxelSizes (float voxelSize) |
Set the voxel size of cubic voxels. The grid data is not updated. More... | |
VoxelGrid () | |
Construct a voxel grid with 0 voxels of size 1.0. More... | |
template<typename OtherVoxelT > | |
VoxelGrid (const VoxelGrid< OtherVoxelT > &other) | |
Construct a voxel grid with the same structure as other. More... | |
VoxelGrid (const VoxelGrid< VoxelT > &other)=default | |
Ordinary copy constructor for voxel grid with same VoxelT. More... | |
VoxelGrid (const VoxelGridStructure &structure, const std::vector< VoxelGrid::VoxelT > &voxelData) | |
Construct a voxel grid with the given structure and data. More... | |
VoxelGrid (const VoxelGridStructure &structure, const VoxelT &value={}) | |
Construct a voxel grid with the given structure and voxels with given value. More... | |
Protected Attributes | |
VoxelGridStructure | _struct |
The geometric structure. More... | |
std::vector< VoxelT > | _voxels |
The voxel data. More... | |
Friends | |
template<class VoxelType > | |
std::ostream & | operator<< (std::ostream &os, const VoxelGrid< VoxelType > &grid) |
Stream a human-readable description of the grid's structure. More... | |
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".
Definition at line 50 of file VoxelGrid.hpp.
using VoxelT = _VoxelT |
The voxel type.
Definition at line 55 of file VoxelGrid.hpp.
Construct a voxel grid with 0 voxels of size 1.0.
Definition at line 456 of file VoxelGrid.hpp.
VoxelGrid | ( | const VoxelGridStructure & | structure, |
const VoxelT & | value = {} |
||
) |
Construct a voxel grid with the given structure and voxels with given value.
Definition at line 460 of file VoxelGrid.hpp.
VoxelGrid | ( | const VoxelGridStructure & | structure, |
const std::vector< VoxelGrid< _VoxelT >::VoxelT > & | voxelData | ||
) |
Construct a voxel grid with the given structure and data.
The size of voxelData
must match the grid size of structure
.
error::InvalidVoxelDataSize | If the size does not match. |
Definition at line 465 of file VoxelGrid.hpp.
Construct a voxel grid with the same structure as other.
Definition at line 478 of file VoxelGrid.hpp.
Ordinary copy constructor for voxel grid with same VoxelT.
|
inline |
Get an iterator to the first voxel.
Definition at line 407 of file VoxelGrid.hpp.
|
inline |
Get an iterator to the first voxel.
Definition at line 412 of file VoxelGrid.hpp.
|
inline |
Get an iterator to the first voxel.
Definition at line 417 of file VoxelGrid.hpp.
|
inline |
Get an iterator to the element following the last voxel.
Definition at line 433 of file VoxelGrid.hpp.
|
inline |
Assert that the given index is a valid grid index.
Definition at line 379 of file VoxelGrid.hpp.
|
inline |
Get an iterator to the element following the last voxel.
Definition at line 423 of file VoxelGrid.hpp.
|
inline |
Get an iterator to the element following the last voxel.
Definition at line 428 of file VoxelGrid.hpp.
|
inline |
Get the geometric center of the grid the world frame (which differs from the origin for even grid sizes).
Definition at line 297 of file VoxelGrid.hpp.
|
inline |
Get the grid pose positioned at the grid center in the world frame.
Definition at line 333 of file VoxelGrid.hpp.
|
inline |
Get extent of the grid along each axis (encompassing the whole voxels).
Definition at line 346 of file VoxelGrid.hpp.
|
inline |
Get extent of the grid along each axis (encompassing only voxel centers).
Definition at line 351 of file VoxelGrid.hpp.
|
inline |
Get the grid size.
Definition at line 177 of file VoxelGrid.hpp.
|
inline |
Get the local axis aligned bounding box of the grid (minimal/maximal values in columns).
Definition at line 356 of file VoxelGrid.hpp.
|
inline |
Get the local axis aligned bounding box of the voxel centers (minimal/maximal values in columns).
Definition at line 361 of file VoxelGrid.hpp.
|
inline |
Get the number of voxels in the grid.
Definition at line 170 of file VoxelGrid.hpp.
|
inline |
Get the grid orienation in the world frame.
Definition at line 283 of file VoxelGrid.hpp.
|
inline |
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.
Definition at line 270 of file VoxelGrid.hpp.
|
inline |
Get the grid pose in the world frame.
Note that this differs from the geometric center for even grid sizes.
Definition at line 320 of file VoxelGrid.hpp.
|
inline |
Get the voxel grid structure.
Definition at line 91 of file VoxelGrid.hpp.
|
inline |
Get the voxel closest to the given point.
local | If false (default), the point is transformed to local coordinate system. |
Definition at line 135 of file VoxelGrid.hpp.
|
inline |
Definition at line 139 of file VoxelGrid.hpp.
|
inline |
Get the voxel with the given grid index.
operator[]
Definition at line 124 of file VoxelGrid.hpp.
|
inline |
Definition at line 128 of file VoxelGrid.hpp.
|
inline |
Get the voxel with the given grid index.
operator[]
Definition at line 114 of file VoxelGrid.hpp.
|
inline |
Definition at line 118 of file VoxelGrid.hpp.
|
inline |
Get the voxel with the given index.
operator[]
Definition at line 104 of file VoxelGrid.hpp.
|
inline |
Definition at line 108 of file VoxelGrid.hpp.
|
inline |
Get the center of the voxel with the given index.
Definition at line 261 of file VoxelGrid.hpp.
|
inline |
Get the center of the voxel with the given index.
Definition at line 256 of file VoxelGrid.hpp.
|
inline |
Get the center of the voxel with the given index.
Definition at line 251 of file VoxelGrid.hpp.
|
inline |
Get the flat index of the voxel closest to point
.
Definition at line 222 of file VoxelGrid.hpp.
|
inline |
Get the flat index of the voxel with given grid index.
Definition at line 217 of file VoxelGrid.hpp.
|
inline |
Get the flat index of the voxel with given grid index.
Definition at line 212 of file VoxelGrid.hpp.
|
inline |
Get the grid index of the voxel closest to point.
local | If false (default), the point is transformed to local coordinate system. |
Definition at line 234 of file VoxelGrid.hpp.
|
inline |
Get the grid index of the voxel with the given flat index.
Definition at line 228 of file VoxelGrid.hpp.
|
inline |
Get the maximal (along each axis) voxel grid index.
Definition at line 245 of file VoxelGrid.hpp.
|
inline |
Get the minimal (along each axis) voxel grid index.
Definition at line 240 of file VoxelGrid.hpp.
|
inline |
Get the voxels.
Definition at line 147 of file VoxelGrid.hpp.
|
inline |
Get the voxel size.
Definition at line 195 of file VoxelGrid.hpp.
|
inline |
Indicate whether the given point is inside the voxel.
Definition at line 373 of file VoxelGrid.hpp.
|
inline |
Indicate whether the given point is inside the voxel.
Definition at line 368 of file VoxelGrid.hpp.
|
inline |
Get the number of voxels in the grid.
Definition at line 165 of file VoxelGrid.hpp.
|
inline |
Get the voxel with given grid index.
getVoxel(const Eigen::Vector3i&)
Definition at line 396 of file VoxelGrid.hpp.
|
inline |
Definition at line 400 of file VoxelGrid.hpp.
|
inline |
Get the voxel with given flat index.
getVoxel(std::size_t)
Definition at line 386 of file VoxelGrid.hpp.
|
inline |
Definition at line 390 of file VoxelGrid.hpp.
|
inline |
Reset the voxel data by numVoxels()
voxels with given value.
Definition at line 84 of file VoxelGrid.hpp.
|
inline |
Set the voxel grid structure and reset voxel data.
Definition at line 96 of file VoxelGrid.hpp.
|
inline |
Set the geometric center of the grid the world frame (which differs from the origin for even grid sizes).
setCenterPose()
.Definition at line 311 of file VoxelGrid.hpp.
|
inline |
Set the grid pose so that the grid center is the position of the given pose under the given orientation.
Definition at line 339 of file VoxelGrid.hpp.
|
inline |
Set the grid size. This resets the grid data.
Definition at line 187 of file VoxelGrid.hpp.
|
inline |
Set the grid size for a cubic grid. Resets the grid data.
Definition at line 182 of file VoxelGrid.hpp.
|
inline |
Set the grid orienation in the world frame.
Definition at line 288 of file VoxelGrid.hpp.
|
inline |
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.
Definition at line 277 of file VoxelGrid.hpp.
|
inline |
Get the grid pose in the world frame.
Note that this differs from the geometric center for even grid sizes.
Definition at line 327 of file VoxelGrid.hpp.
|
inline |
Set the voxels.
(Size must match numVoxels()
).
error::InvalidVoxelDataSize | If the size does not match. |
Definition at line 154 of file VoxelGrid.hpp.
|
inline |
Set the voxel size. The grid data is not updated.
Definition at line 205 of file VoxelGrid.hpp.
|
inline |
Set the voxel size of cubic voxels. The grid data is not updated.
Definition at line 200 of file VoxelGrid.hpp.
|
friend |
Stream a human-readable description of the grid's structure.
|
protected |
The geometric structure.
Definition at line 447 of file VoxelGrid.hpp.
|
protected |
The voxel data.
Definition at line 450 of file VoxelGrid.hpp.