moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
distance_field::VoxelGrid< T > Class Template Reference

VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplied as a template parameter. More...

#include <voxel_grid.h>

Public Member Functions

 MOVEIT_DECLARE_PTR_MEMBER (VoxelGrid)
 
 VoxelGrid (double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, T default_object)
 Constructor for the VoxelGrid.
 
virtual ~VoxelGrid ()
 
 VoxelGrid ()
 Default constructor for the VoxelGrid.
 
void resize (double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, T default_object)
 Resize the VoxelGrid.
 
const T & operator() (double x, double y, double z) const
 Operator that gets the value of the given location (x, y, z) given the discretization of the volume. The location represents a location in the original coordinate frame used to construct the voxel grid.
 
const T & operator() (const Eigen::Vector3d &pos) const
 
T & getCell (int x, int y, int z)
 Gives the value of the given location (x,y,z) in the discretized voxel grid space.
 
T & getCell (const Eigen::Vector3i &pos)
 
const T & getCell (int x, int y, int z) const
 
const T & getCell (const Eigen::Vector3i &pos) const
 
void setCell (int x, int y, int z, const T &obj)
 Sets the value of the given location (x,y,z) in the discretized voxel grid space to supplied value.
 
void setCell (const Eigen::Vector3i &pos, const T &obj)
 
void reset (const T &initial)
 Sets every cell in the voxel grid to the supplied data.
 
double getSize (Dimension dim) const
 Gets the size in arbitrary units of the indicated dimension.
 
double getResolution () const
 Gets the resolution in arbitrary consistent units.
 
double getOrigin (Dimension dim) const
 Gets the origin (minimum point) of the indicated dimension.
 
int getNumCells (Dimension dim) const
 Gets the number of cells in the indicated dimension.
 
void gridToWorld (int x, int y, int z, double &world_x, double &world_y, double &world_z) const
 Converts grid coordinates to world coordinates.
 
void gridToWorld (const Eigen::Vector3i &grid, Eigen::Vector3d &world) const
 
bool worldToGrid (double world_x, double world_y, double world_z, int &x, int &y, int &z) const
 Converts from a world location to a set of integer indices. Does check whether or not the cell being returned is valid. The returned indices will be computed even if they are invalid.
 
bool worldToGrid (const Eigen::Vector3d &world, Eigen::Vector3i &grid) const
 
bool isCellValid (int x, int y, int z) const
 Checks if the given cell in integer coordinates is within the voxel grid.
 
bool isCellValid (const Eigen::Vector3i &pos) const
 
bool isCellValid (Dimension dim, int cell) const
 Checks if the indicated index is valid along a particular dimension.
 

Protected Member Functions

int ref (int x, int y, int z) const
 Gets the 1D index into the array, with no validity check.
 
int getCellFromLocation (Dimension dim, double loc) const
 Gets the cell number from the location.
 
double getLocationFromCell (Dimension dim, int cell) const
 Gets the center of the cell in world coordinates along the given dimension. No validity check.
 

Protected Attributes

T * data_
 Storage for the full set of data elements.
 
default_object_
 The default object to return in case of out-of-bounds query.
 
T *** data_ptrs_
 3D array of pointers to the data elements
 
double size_ [3]
 The size of each dimension in meters (in Dimension order)
 
double resolution_
 The resolution of each dimension in meters (in Dimension order)
 
double oo_resolution_
 1.0/resolution_
 
double origin_ [3]
 The origin (minimum point) of each dimension in meters (in Dimension order)
 
double origin_minus_ [3]
 origin - 0.5/resolution
 
int num_cells_ [3]
 The number of cells in each dimension (in Dimension order)
 
int num_cells_total_
 The total number of voxels in the grid.
 
int stride1_
 The step to take when stepping between consecutive X members in the 1D array.
 
int stride2_
 The step to take when stepping between consecutive Y members given an X in the 1D array.
 

Detailed Description

template<typename T>
class distance_field::VoxelGrid< T >

VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplied as a template parameter.

Definition at line 61 of file voxel_grid.h.

Constructor & Destructor Documentation

◆ VoxelGrid() [1/2]

template<typename T >
distance_field::VoxelGrid< T >::VoxelGrid ( double  size_x,
double  size_y,
double  size_z,
double  resolution,
double  origin_x,
double  origin_y,
double  origin_z,
default_object 
)

Constructor for the VoxelGrid.

Constructs a dense representation of a 3D, axis-aligned volume at a given resolution. The volume can be represented in any consistent set of units, but for the sake of documentation we assume that the units are meters. The size of the the volume is given along each of the X, Y, and Z axes. The volume begins at the minimum point in each dimension, as specified by the origin parameters. The data structure will remain uninitialized until the VoxelGrid::reset function is called.

Parameters
[in]size_xSize of the X axis in meters
[in]size_ySize of the Y axis in meters
[in]size_zSize of the Z axis in meters
[in]resolutionResolution of a single cell in meters
[in]origin_xMinimum point along the X axis of the volume
[in]origin_yMinimum point along the Y axis of the volume
[in]origin_zMinimum point along the Z axis of the volume
[in]default_objectAn object that will be returned for any future queries that are not valid

Definition at line 339 of file voxel_grid.h.

Here is the call graph for this function:

◆ ~VoxelGrid()

template<typename T >
distance_field::VoxelGrid< T >::~VoxelGrid ( )
virtual

Definition at line 399 of file voxel_grid.h.

◆ VoxelGrid() [2/2]

template<typename T >
distance_field::VoxelGrid< T >::VoxelGrid ( )

Default constructor for the VoxelGrid.

This is only useful if resize() is called after construction to set the size and resolution of the VoxelGrid.

Definition at line 347 of file voxel_grid.h.

Member Function Documentation

◆ getCell() [1/4]

template<typename T >
T & distance_field::VoxelGrid< T >::getCell ( const Eigen::Vector3i &  pos)
inline

Definition at line 482 of file voxel_grid.h.

◆ getCell() [2/4]

template<typename T >
const T & distance_field::VoxelGrid< T >::getCell ( const Eigen::Vector3i &  pos) const
inline

Definition at line 488 of file voxel_grid.h.

◆ getCell() [3/4]

template<typename T >
T & distance_field::VoxelGrid< T >::getCell ( int  x,
int  y,
int  z 
)
inline

Gives the value of the given location (x,y,z) in the discretized voxel grid space.

The address here is in the discretized space of the voxel grid, where the cell indicated by the constructor arguments (origin_x, origin_y, origin_z) is cell (0,0,0), and the cell indicated by (origin_x+x_size, origin_y+y_size, origin_z+z_size) will be (size_x/resolution, size_y/resolution, size_z/resolution).

Parameters
[in]xThe X index of the desired cell
[in]yThe Y index of the desired cell
[in]zThe Z index of the desired cell
Returns
The data in the indicated cell. If x,y,z is invalid then corruption and/or SEGFAULTS will occur.

Definition at line 470 of file voxel_grid.h.

Here is the caller graph for this function:

◆ getCell() [4/4]

template<typename T >
const T & distance_field::VoxelGrid< T >::getCell ( int  x,
int  y,
int  z 
) const
inline

Definition at line 476 of file voxel_grid.h.

◆ getCellFromLocation()

template<typename T >
int distance_field::VoxelGrid< T >::getCellFromLocation ( Dimension  dim,
double  loc 
) const
inlineprotected

Gets the cell number from the location.

Gets the cell number in a given dimension given a world value. No validity check.

Parameters
[in]dimThe dimension of the query
[in]locThe world location along that dimension
Returns
The computed cell index along the given dimension

Definition at line 506 of file voxel_grid.h.

◆ getLocationFromCell()

template<typename T >
double distance_field::VoxelGrid< T >::getLocationFromCell ( Dimension  dim,
int  cell 
) const
inlineprotected

Gets the center of the cell in world coordinates along the given dimension. No validity check.

Parameters
[in]dimThe dimension of the query
[in]cellThe cell along the given dimension
Returns
The world coordinate of the center of the cell

Definition at line 526 of file voxel_grid.h.

◆ getNumCells()

template<typename T >
int distance_field::VoxelGrid< T >::getNumCells ( Dimension  dim) const
inline

Gets the number of cells in the indicated dimension.

Parameters
[in]dimThe dimension for the query
Returns
The number of cells for the indicated dimension

Definition at line 447 of file voxel_grid.h.

Here is the caller graph for this function:

◆ getOrigin()

template<typename T >
double distance_field::VoxelGrid< T >::getOrigin ( Dimension  dim) const
inline

Gets the origin (minimum point) of the indicated dimension.

Parameters
[in]dimThe dimension for the query
Returns
The indicated axis origin

Definition at line 441 of file voxel_grid.h.

◆ getResolution()

template<typename T >
double distance_field::VoxelGrid< T >::getResolution ( ) const
inline

Gets the resolution in arbitrary consistent units.

Returns
The resolution in meters

Definition at line 435 of file voxel_grid.h.

◆ getSize()

template<typename T >
double distance_field::VoxelGrid< T >::getSize ( Dimension  dim) const
inline

Gets the size in arbitrary units of the indicated dimension.

Parameters
[in]dimThe dimension for the query
Returns
The size in meters

Definition at line 429 of file voxel_grid.h.

◆ gridToWorld() [1/2]

template<typename T >
void distance_field::VoxelGrid< T >::gridToWorld ( const Eigen::Vector3i &  grid,
Eigen::Vector3d &  world 
) const
inline

Definition at line 546 of file voxel_grid.h.

◆ gridToWorld() [2/2]

template<typename T >
void distance_field::VoxelGrid< T >::gridToWorld ( int  x,
int  y,
int  z,
double &  world_x,
double &  world_y,
double &  world_z 
) const
inline

Converts grid coordinates to world coordinates.

Converts from an set of integer indices to a world location given the origin and resolution parameters. There is no check whether or not the cell or world locations lie within the represented region.

Parameters
[in]xThe integer X location
[in]yThe integer Y location
[in]zThe integer Z location
[out]world_xThe computed world X location
[out]world_yThe computed world X location
[out]world_zThe computed world X location
Returns
True, as there is no check that the integer locations are valid

Definition at line 538 of file voxel_grid.h.

◆ isCellValid() [1/3]

template<typename T >
bool distance_field::VoxelGrid< T >::isCellValid ( const Eigen::Vector3i &  pos) const
inline

Definition at line 411 of file voxel_grid.h.

◆ isCellValid() [2/3]

template<typename T >
bool distance_field::VoxelGrid< T >::isCellValid ( Dimension  dim,
int  cell 
) const
inline

Checks if the indicated index is valid along a particular dimension.

Parameters
[in]dimThe dimension for the query
[in]cellThe index along that dimension
Returns
True if the cell is valid along that dimension; otherwise False.

Definition at line 417 of file voxel_grid.h.

◆ isCellValid() [3/3]

template<typename T >
bool distance_field::VoxelGrid< T >::isCellValid ( int  x,
int  y,
int  z 
) const
inline

Checks if the given cell in integer coordinates is within the voxel grid.

Parameters
[in]xThe integer X location
[in]yThe integer Y location
[in]zThe integer Z location
Returns
True if the cell lies within the voxel grid; otherwise False.

Definition at line 405 of file voxel_grid.h.

◆ MOVEIT_DECLARE_PTR_MEMBER()

template<typename T >
distance_field::VoxelGrid< T >::MOVEIT_DECLARE_PTR_MEMBER ( VoxelGrid< T >  )

◆ operator()() [1/2]

template<typename T >
const T & distance_field::VoxelGrid< T >::operator() ( const Eigen::Vector3d &  pos) const
inline

Definition at line 464 of file voxel_grid.h.

◆ operator()() [2/2]

template<typename T >
const T & distance_field::VoxelGrid< T >::operator() ( double  x,
double  y,
double  z 
) const
inline

Operator that gets the value of the given location (x, y, z) given the discretization of the volume. The location represents a location in the original coordinate frame used to construct the voxel grid.

Parameters
[in]xX component of the desired location
[in]yY component of the desired location
[in]zZ component of the desired location
Returns
The data stored at that location, or a default value supplied in the constructor if the location is not valid.

Definition at line 453 of file voxel_grid.h.

◆ ref()

template<typename T >
int distance_field::VoxelGrid< T >::ref ( int  x,
int  y,
int  z 
) const
inlineprotected

Gets the 1D index into the array, with no validity check.

Parameters
[in]xThe integer X location
[in]yThe integer Y location
[in]zThe integer Z location
Returns
The computed 1D index

Definition at line 423 of file voxel_grid.h.

◆ reset()

template<typename T >
void distance_field::VoxelGrid< T >::reset ( const T &  initial)
inline

Sets every cell in the voxel grid to the supplied data.

Parameters
[in]initialThe template variable to which to set the data

Definition at line 532 of file voxel_grid.h.

Here is the caller graph for this function:

◆ resize()

template<typename T >
void distance_field::VoxelGrid< T >::resize ( double  size_x,
double  size_y,
double  size_z,
double  resolution,
double  origin_x,
double  origin_y,
double  origin_z,
default_object 
)

Resize the VoxelGrid.

This discards all the data in the voxel grid and reinitializes it with a new size and resolution. This is mainly useful if the size or resolution is not known until after the voxelgrid is constructed.

Parameters
[in]size_xSize of the X axis in meters
[in]size_ySize of the Y axis in meters
[in]size_zSize of the Z axis in meters
[in]resolutionResolution of a single cell in meters
[in]origin_xMinimum point along the X axis of the volume
[in]origin_yMinimum point along the Y axis of the volume
[in]origin_zMinimum point along the Z axis of the volume

Definition at line 364 of file voxel_grid.h.

Here is the caller graph for this function:

◆ setCell() [1/2]

template<typename T >
void distance_field::VoxelGrid< T >::setCell ( const Eigen::Vector3i &  pos,
const T &  obj 
)
inline

Definition at line 500 of file voxel_grid.h.

◆ setCell() [2/2]

template<typename T >
void distance_field::VoxelGrid< T >::setCell ( int  x,
int  y,
int  z,
const T &  obj 
)
inline

Sets the value of the given location (x,y,z) in the discretized voxel grid space to supplied value.

The address here is in the discretized space of the voxel grid, where the cell indicated by the constructor arguments (origin_x, origin_y, origin_z) is cell (0,0,0), and the cell indicated by (origin_x+x_size, origin_y+y_size, origin_z+z_size) will be (size_x/resolution, size_y/resolution, size_z/resolution).

If the arguments do not indicate a valid cell, corruption and/or SEGFAULTS will occur.

Parameters
[in]xThe X index of the desired cell
[in]yThe Y index of the desired cell
[in]zThe Z index of the desired cell
[out]objThe data to place into the given cell

Definition at line 494 of file voxel_grid.h.

◆ worldToGrid() [1/2]

template<typename T >
bool distance_field::VoxelGrid< T >::worldToGrid ( const Eigen::Vector3d &  world,
Eigen::Vector3i &  grid 
) const
inline

Definition at line 563 of file voxel_grid.h.

◆ worldToGrid() [2/2]

template<typename T >
bool distance_field::VoxelGrid< T >::worldToGrid ( double  world_x,
double  world_y,
double  world_z,
int &  x,
int &  y,
int &  z 
) const
inline

Converts from a world location to a set of integer indices. Does check whether or not the cell being returned is valid. The returned indices will be computed even if they are invalid.

Parameters
[in]world_xThe world X location
[in]world_yThe world Y location
[in]world_zThe world Z location
[out]xThe computed integer X location
[out]yThe computed integer X location
[out]zThe computed integer X location
Returns
True if all the world values result in integer indices that pass a validity check; otherwise False.

Definition at line 554 of file voxel_grid.h.

Member Data Documentation

◆ data_

template<typename T >
T* distance_field::VoxelGrid< T >::data_
protected

Storage for the full set of data elements.

Definition at line 286 of file voxel_grid.h.

◆ data_ptrs_

template<typename T >
T*** distance_field::VoxelGrid< T >::data_ptrs_
protected

3D array of pointers to the data elements

Definition at line 288 of file voxel_grid.h.

◆ default_object_

template<typename T >
T distance_field::VoxelGrid< T >::default_object_
protected

The default object to return in case of out-of-bounds query.

Definition at line 287 of file voxel_grid.h.

◆ num_cells_

template<typename T >
int distance_field::VoxelGrid< T >::num_cells_[3]
protected

The number of cells in each dimension (in Dimension order)

Definition at line 294 of file voxel_grid.h.

◆ num_cells_total_

template<typename T >
int distance_field::VoxelGrid< T >::num_cells_total_
protected

The total number of voxels in the grid.

Definition at line 295 of file voxel_grid.h.

◆ oo_resolution_

template<typename T >
double distance_field::VoxelGrid< T >::oo_resolution_
protected

1.0/resolution_

Definition at line 291 of file voxel_grid.h.

◆ origin_

template<typename T >
double distance_field::VoxelGrid< T >::origin_[3]
protected

The origin (minimum point) of each dimension in meters (in Dimension order)

Definition at line 292 of file voxel_grid.h.

◆ origin_minus_

template<typename T >
double distance_field::VoxelGrid< T >::origin_minus_[3]
protected

origin - 0.5/resolution

Definition at line 293 of file voxel_grid.h.

◆ resolution_

template<typename T >
double distance_field::VoxelGrid< T >::resolution_
protected

The resolution of each dimension in meters (in Dimension order)

Definition at line 290 of file voxel_grid.h.

◆ size_

template<typename T >
double distance_field::VoxelGrid< T >::size_[3]
protected

The size of each dimension in meters (in Dimension order)

Definition at line 289 of file voxel_grid.h.

◆ stride1_

template<typename T >
int distance_field::VoxelGrid< T >::stride1_
protected

The step to take when stepping between consecutive X members in the 1D array.

Definition at line 296 of file voxel_grid.h.

◆ stride2_

template<typename T >
int distance_field::VoxelGrid< T >::stride2_
protected

The step to take when stepping between consecutive Y members given an X in the 1D array.

Definition at line 297 of file voxel_grid.h.


The documentation for this class was generated from the following file: