37 #ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_ 38 #define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_ 121 void resize(
double size_x,
double size_y,
double size_z,
double resolution,
double origin_x,
double origin_y,
122 double origin_z, T default_object);
138 const T&
operator()(
const Eigen::Vector3d& pos)
const;
157 T&
getCell(
int x,
int y,
int z);
158 T&
getCell(
const Eigen::Vector3i& pos);
159 const T&
getCell(
int x,
int y,
int z)
const;
160 const T&
getCell(
const Eigen::Vector3i& pos)
const;
180 void setCell(
int x,
int y,
int z,
const T& obj);
181 void setCell(
const Eigen::Vector3i& pos,
const T& obj);
188 void reset(
const T& initial);
245 void gridToWorld(
int x,
int y,
int z,
double& world_x,
double& world_y,
double& world_z)
const;
246 void gridToWorld(
const Eigen::Vector3i& grid, Eigen::Vector3i& world)
const;
264 bool worldToGrid(
double world_x,
double world_y,
double world_z,
int& x,
int& y,
int& z)
const;
265 bool worldToGrid(
const Eigen::Vector3i& world, Eigen::Vector3i& grid)
const;
277 bool isCellValid(
const Eigen::Vector3i& pos)
const;
312 int ref(
int x,
int y,
int z)
const;
342 template <
typename T>
347 resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
350 template <
typename T>
367 template <
typename T>
402 template <
typename T>
408 template <
typename T>
414 template <
typename T>
420 template <
typename T>
426 template <
typename T>
432 template <
typename T>
438 template <
typename T>
444 template <
typename T>
450 template <
typename T>
456 template <
typename T>
462 template <
typename T>
470 return getCell(cellX, cellY, cellZ);
473 template <
typename T>
476 return this->
operator()(pos.x(), pos.y(), pos.z());
479 template <
typename T>
485 template <
typename T>
491 template <
typename T>
494 return data_[
ref(pos.x(), pos.y(), pos.z())];
497 template <
typename T>
500 return data_[
ref(pos.x(), pos.y(), pos.z())];
503 template <
typename T>
509 template <
typename T>
512 data_[
ref(pos.x(), pos.y(), pos.z())] = obj;
515 template <
typename T>
535 template <
typename T>
541 template <
typename T>
547 template <
typename T>
555 template <
typename T>
563 template <
typename T>
572 template <
typename T>
double getLocationFromCell(Dimension dim, int cell) const
Gets the center of the cell in world coordinates along the given dimension. No validity check...
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 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.
double origin_[3]
The origin (minumum point) of each dimension in meters (in Dimension order)
int stride1_
The step to take when stepping between consecutive X members in the 1D array.
double getOrigin(Dimension dim) const
Gets the origin (miniumum point) of the indicated dimension.
double origin_minus_[3]
origin - 0.5/resolution
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...
double getSize(Dimension dim) const
Gets the size in arbitrary units of the indicated dimension.
static const double origin_y
VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplie...
double size_[3]
The size of each dimension in meters (in Dimension order)
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...
int stride2_
The step to take when stepping between consecutive Y members given an X in the 1D array...
int getCellFromLocation(Dimension dim, double loc) const
Gets the cell number from the location.
bool isCellValid(int x, int y, int z) const
Checks if the given cell in integer coordinates is within the voxel grid.
int num_cells_[3]
The number of cells in each dimension (in Dimension order)
static const double resolution
T * data_
Storage for the full set of data elements.
T *** data_ptrs_
3D array of pointers to the data elements
static const double origin_z
double resolution_
The resolution of each dimension in meters (in Dimension order)
T default_object_
The default object to return in case of out-of-bounds query.
int getNumCells(Dimension dim) const
Gets the number of cells in the indicated dimension.
double oo_resolution_
1.0/resolution_
double getResolution() const
Gets the resolution in arbitrary consistent units.
Namespace for holding classes that generate distance fields.
Dimension
Specifies dimension of different axes.
MOVEIT_DECLARE_PTR_MEMBER(VoxelGrid)
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 ...
int num_cells_total_
The total number of voxels in the grid.
void reset(const T &initial)
Sets every cell in the voxel grid to the supplied data.
VoxelGrid()
Default constructor for the VoxelGrid.
int ref(int x, int y, int z) const
Gets the 1D index into the array, with no validity check.
T & getCell(int x, int y, int z)
Gives the value of the given location (x,y,z) in the discretized voxel grid space.
static const double origin_x