91 VoxelGrid(
double size_x,
double size_y,
double size_z,
double resolution,
double origin_x,
double origin_y,
92 double origin_z, T default_object);
120 void resize(
double size_x,
double size_y,
double size_z,
double resolution,
double origin_x,
double origin_y,
121 double origin_z, T default_object);
159 const T&
getCell(
const Eigen::Vector3i& pos)
const;
179 void setCell(
int x,
int y,
int z,
const T& obj);
180 void setCell(
const Eigen::Vector3i& pos,
const T& obj);
241 void gridToWorld(
int x,
int y,
int z,
double& world_x,
double& world_y,
double& world_z)
const;
260 bool worldToGrid(
double world_x,
double world_y,
double world_z,
int& x,
int& y,
int& z)
const;
308 int ref(
int x,
int y,
int z)
const;
338 template <
typename T>
340 double origin_y,
double origin_z, T default_object)
343 resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
346 template <
typename T>
363 template <
typename T>
365 double origin_y,
double origin_z, T default_object)
370 size_[
DIM_X] = size_x;
371 size_[
DIM_Y] = size_y;
372 size_[
DIM_Z] = size_z;
373 origin_[
DIM_X] = origin_x;
374 origin_[
DIM_Y] = origin_y;
375 origin_[
DIM_Z] = origin_z;
376 origin_minus_[
DIM_X] = origin_x - 0.5 * resolution;
377 origin_minus_[
DIM_Y] = origin_y - 0.5 * resolution;
378 origin_minus_[
DIM_Z] = origin_z - 0.5 * resolution;
379 num_cells_total_ = 1;
380 resolution_ = resolution;
381 oo_resolution_ = 1.0 / resolution_;
384 num_cells_[i] = size_[i] * oo_resolution_;
385 num_cells_total_ *= num_cells_[i];
388 default_object_ = default_object;
390 stride1_ = num_cells_[
DIM_Y] * num_cells_[
DIM_Z];
391 stride2_ = num_cells_[
DIM_Z];
394 if (num_cells_total_ > 0)
395 data_ =
new T[num_cells_total_];
398 template <
typename T>
404 template <
typename T>
407 return (x >= 0 && x < num_cells_[
DIM_X] && y >= 0 && y < num_cells_[
DIM_Y] && z >= 0 && z < num_cells_[
DIM_Z]);
410 template <
typename T>
413 return isCellValid(pos.x(), pos.y(), pos.z());
416 template <
typename T>
419 return cell >= 0 && cell < num_cells_[dim];
422 template <
typename T>
425 return x * stride1_ + y * stride2_ + z;
428 template <
typename T>
434 template <
typename T>
440 template <
typename T>
446 template <
typename T>
449 return num_cells_[dim];
452 template <
typename T>
455 int cell_x = getCellFromLocation(
DIM_X, x);
456 int cell_y = getCellFromLocation(
DIM_Y, y);
457 int cell_z = getCellFromLocation(
DIM_Z, z);
458 if (!isCellValid(cell_x, cell_y, cell_z))
459 return default_object_;
460 return getCell(cell_x, cell_y, cell_z);
463 template <
typename T>
466 return operator()(pos.x(), pos.y(), pos.z());
469 template <
typename T>
472 return data_[ref(x, y, z)];
475 template <
typename T>
478 return data_[ref(x, y, z)];
481 template <
typename T>
484 return data_[ref(pos.x(), pos.y(), pos.z())];
487 template <
typename T>
490 return data_[ref(pos.x(), pos.y(), pos.z())];
493 template <
typename T>
496 data_[ref(x, y, z)] = obj;
499 template <
typename T>
502 data_[ref(pos.x(), pos.y(), pos.z())] = obj;
505 template <
typename T>
522 return int(floor((loc - origin_minus_[dim]) * oo_resolution_));
525 template <
typename T>
528 return origin_[dim] + resolution_ * (double(cell));
531 template <
typename T>
534 std::fill(data_, data_ + num_cells_total_, initial);
537 template <
typename T>
540 world_x = getLocationFromCell(
DIM_X, x);
541 world_y = getLocationFromCell(
DIM_Y, y);
542 world_z = getLocationFromCell(
DIM_Z, z);
545 template <
typename T>
548 world.x() = getLocationFromCell(
DIM_X, grid.x());
549 world.y() = getLocationFromCell(
DIM_Y, grid.y());
550 world.z() = getLocationFromCell(
DIM_Z, grid.z());
553 template <
typename T>
556 x = getCellFromLocation(
DIM_X, world_x);
557 y = getCellFromLocation(
DIM_Y, world_y);
558 z = getCellFromLocation(
DIM_Z, world_z);
559 return isCellValid(x, y, z);
562 template <
typename T>
565 grid.x() = getCellFromLocation(
DIM_X, world.x());
566 grid.y() = getCellFromLocation(
DIM_Y, world.y());
567 grid.z() = getCellFromLocation(
DIM_Z, world.z());
568 return isCellValid(grid.x(), grid.y(), grid.z());
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)
int stride2_
The step to take when stepping between consecutive Y members given an X in the 1D array.
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 ...
VoxelGrid()
Default constructor for 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....
double oo_resolution_
1.0/resolution_
T *** data_ptrs_
3D array of pointers to the data elements
int num_cells_total_
The total number of voxels in the grid.
int getNumCells(Dimension dim) const
Gets the number of cells in the indicated dimension.
int getCellFromLocation(Dimension dim, double loc) const
Gets the cell number from the location.
void setCell(const Eigen::Vector3i &pos, const T &obj)
double origin_minus_[3]
origin - 0.5/resolution
const T & getCell(const Eigen::Vector3i &pos) const
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.
double origin_[3]
The origin (minimum 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.
T & getCell(int x, int y, int z)
Gives the value of the given location (x,y,z) in the discretized voxel grid space.
double resolution_
The resolution 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.
void reset(const T &initial)
Sets every cell in the voxel grid to the supplied data.
double getLocationFromCell(Dimension dim, int cell) const
Gets the center of the cell in world coordinates along the given dimension. No validity check.
MOVEIT_DECLARE_PTR_MEMBER(VoxelGrid)
T & getCell(const Eigen::Vector3i &pos)
const T & operator()(const Eigen::Vector3d &pos) const
int num_cells_[3]
The number of cells in each dimension (in Dimension order)
double getOrigin(Dimension dim) const
Gets the origin (minimum point) of the indicated dimension.
const T & getCell(int x, int y, int z) const
T * data_
Storage for the full set of data elements.
bool isCellValid(int x, int y, int z) const
Checks if the given cell in integer coordinates is within the voxel grid.
T default_object_
The default object to return in case of out-of-bounds query.
bool worldToGrid(const Eigen::Vector3d &world, Eigen::Vector3i &grid) const
double getSize(Dimension dim) const
Gets the size in arbitrary units of the indicated dimension.
void gridToWorld(const Eigen::Vector3i &grid, Eigen::Vector3d &world) const
int ref(int x, int y, int z) const
Gets the 1D index into the array, with no validity check.
double getResolution() const
Gets the resolution in arbitrary consistent units.
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.
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.
void gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const
Converts grid coordinates to world coordinates.
Namespace for holding classes that generate distance fields.
Dimension
Specifies dimension of different axes.
Vec3fX< details::Vec3Data< double > > Vector3d