voxel_grid.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan, Acorn Pooley */
36 
37 #ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
38 #define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
39 
40 #include <algorithm>
41 #include <cmath>
42 #include <Eigen/Core>
44 
45 namespace distance_field
46 {
49 {
50  DIM_X = 0,
51  DIM_Y = 1,
52  DIM_Z = 2
53 };
54 
61 template <typename T>
62 class VoxelGrid
63 {
64 public:
66 
92  VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
93  double origin_z, T default_object);
94  virtual ~VoxelGrid();
95 
102  VoxelGrid();
103 
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);
123 
137  const T& operator()(double x, double y, double z) const;
138  const T& operator()(const Eigen::Vector3d& pos) const;
139 
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;
161 
180  void setCell(int x, int y, int z, const T& obj);
181  void setCell(const Eigen::Vector3i& pos, const T& obj);
182 
188  void reset(const T& initial);
189 
197  double getSize(Dimension dim) const;
198 
204  double getResolution() const;
205 
207  double getResolution(Dimension dim) const;
208 
216  double getOrigin(Dimension dim) const;
217 
225  int getNumCells(Dimension dim) const;
226 
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;
247 
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;
266 
276  bool isCellValid(int x, int y, int z) const;
277  bool isCellValid(const Eigen::Vector3i& pos) const;
278 
287  bool isCellValid(Dimension dim, int cell) const;
288 
289 protected:
290  T* data_;
292  T*** data_ptrs_;
293  double size_[3];
294  double resolution_;
295  double oo_resolution_;
296  double origin_[3];
297  double origin_minus_[3];
298  int num_cells_[3];
300  int stride1_;
301  int stride2_;
312  int ref(int x, int y, int z) const;
313 
326  int getCellFromLocation(Dimension dim, double loc) const;
327 
337  double getLocationFromCell(Dimension dim, int cell) const;
338 };
339 
341 
342 template <typename T>
343 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x,
344  double origin_y, double origin_z, T default_object)
345  : data_(NULL)
346 {
347  resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
348 }
349 
350 template <typename T>
352 {
353  for (int i = DIM_X; i <= DIM_Z; ++i)
354  {
355  size_[i] = 0;
356  origin_[i] = 0;
357  origin_minus_[i] = 0;
358  num_cells_[i] = 0;
359  }
360  resolution_ = 1.0;
361  oo_resolution_ = 1.0 / resolution_;
362  num_cells_total_ = 0;
363  stride1_ = 0;
364  stride2_ = 0;
365 }
366 
367 template <typename T>
368 void VoxelGrid<T>::resize(double size_x, double size_y, double size_z, double resolution, double origin_x,
369  double origin_y, double origin_z, T default_object)
370 {
371  delete[] data_;
372  data_ = NULL;
373 
374  size_[DIM_X] = size_x;
375  size_[DIM_Y] = size_y;
376  size_[DIM_Z] = size_z;
380  origin_minus_[DIM_X] = origin_x - 0.5 * resolution;
381  origin_minus_[DIM_Y] = origin_y - 0.5 * resolution;
382  origin_minus_[DIM_Z] = origin_z - 0.5 * resolution;
383  num_cells_total_ = 1;
385  oo_resolution_ = 1.0 / resolution_;
386  for (int i = DIM_X; i <= DIM_Z; ++i)
387  {
388  num_cells_[i] = size_[i] * oo_resolution_;
390  }
391 
392  default_object_ = default_object;
393 
395  stride2_ = num_cells_[DIM_Z];
396 
397  // initialize the data:
398  if (num_cells_total_ > 0)
399  data_ = new T[num_cells_total_];
400 }
401 
402 template <typename T>
404 {
405  delete[] data_;
406 }
407 
408 template <typename T>
409 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
410 {
411  return (x >= 0 && x < num_cells_[DIM_X] && y >= 0 && y < num_cells_[DIM_Y] && z >= 0 && z < num_cells_[DIM_Z]);
412 }
413 
414 template <typename T>
415 inline bool VoxelGrid<T>::isCellValid(const Eigen::Vector3i& pos) const
416 {
417  return isCellValid(pos.x(), pos.y(), pos.z());
418 }
419 
420 template <typename T>
421 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
422 {
423  return cell >= 0 && cell < num_cells_[dim];
424 }
425 
426 template <typename T>
427 inline int VoxelGrid<T>::ref(int x, int y, int z) const
428 {
429  return x * stride1_ + y * stride2_ + z;
430 }
431 
432 template <typename T>
433 inline double VoxelGrid<T>::getSize(Dimension dim) const
434 {
435  return size_[dim];
436 }
437 
438 template <typename T>
439 inline double VoxelGrid<T>::getResolution() const
440 {
441  return resolution_;
442 }
443 
444 template <typename T>
445 inline double VoxelGrid<T>::getResolution(Dimension dim) const
446 {
447  return resolution_;
448 }
449 
450 template <typename T>
451 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
452 {
453  return origin_[dim];
454 }
455 
456 template <typename T>
458 {
459  return num_cells_[dim];
460 }
461 
462 template <typename T>
463 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
464 {
465  int cellX = getCellFromLocation(DIM_X, x);
466  int cellY = getCellFromLocation(DIM_Y, y);
467  int cellZ = getCellFromLocation(DIM_Z, z);
468  if (!isCellValid(cellX, cellY, cellZ))
469  return default_object_;
470  return getCell(cellX, cellY, cellZ);
471 }
472 
473 template <typename T>
474 inline const T& VoxelGrid<T>::operator()(const Eigen::Vector3d& pos) const
475 {
476  return this->operator()(pos.x(), pos.y(), pos.z());
477 }
478 
479 template <typename T>
480 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
481 {
482  return data_[ref(x, y, z)];
483 }
484 
485 template <typename T>
486 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
487 {
488  return data_[ref(x, y, z)];
489 }
490 
491 template <typename T>
492 inline T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos)
493 {
494  return data_[ref(pos.x(), pos.y(), pos.z())];
495 }
496 
497 template <typename T>
498 inline const T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos) const
499 {
500  return data_[ref(pos.x(), pos.y(), pos.z())];
501 }
502 
503 template <typename T>
504 inline void VoxelGrid<T>::setCell(int x, int y, int z, const T& obj)
505 {
506  data_[ref(x, y, z)] = obj;
507 }
508 
509 template <typename T>
510 inline void VoxelGrid<T>::setCell(const Eigen::Vector3i& pos, const T& obj)
511 {
512  data_[ref(pos.x(), pos.y(), pos.z())] = obj;
513 }
514 
515 template <typename T>
516 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
517 {
518  // This implements
519  //
520  // ( loc - origin )
521  // floor( ------------ + 0.5 )
522  // ( resolution )
523  //
524  // In other words, the rounded quantized location.
525  //
526  // For speed implemented like this:
527  //
528  // floor( ( loc - origin_minus ) * oo_resolution )
529  //
530  // where origin_minus = origin - 0.5*resolution
531  //
532  return int(floor((loc - origin_minus_[dim]) * oo_resolution_));
533 }
534 
535 template <typename T>
536 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
537 {
538  return origin_[dim] + resolution_ * (double(cell));
539 }
540 
541 template <typename T>
542 inline void VoxelGrid<T>::reset(const T& initial)
543 {
544  std::fill(data_, data_ + num_cells_total_, initial);
545 }
546 
547 template <typename T>
548 inline void VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
549 {
550  world_x = getLocationFromCell(DIM_X, x);
551  world_y = getLocationFromCell(DIM_Y, y);
552  world_z = getLocationFromCell(DIM_Z, z);
553 }
554 
555 template <typename T>
556 inline void VoxelGrid<T>::gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3i& world) const
557 {
558  world.x() = getLocationFromCell(DIM_X, grid.x());
559  world.y() = getLocationFromCell(DIM_Y, grid.y());
560  world.z() = getLocationFromCell(DIM_Z, grid.z());
561 }
562 
563 template <typename T>
564 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
565 {
566  x = getCellFromLocation(DIM_X, world_x);
567  y = getCellFromLocation(DIM_Y, world_y);
568  z = getCellFromLocation(DIM_Z, world_z);
569  return isCellValid(x, y, z);
570 }
571 
572 template <typename T>
573 inline bool VoxelGrid<T>::worldToGrid(const Eigen::Vector3i& world, Eigen::Vector3i& grid) const
574 {
575  grid.x() = getCellFromLocation(DIM_X, world.x());
576  grid.y() = getCellFromLocation(DIM_Y, world.y());
577  grid.z() = getCellFromLocation(DIM_Z, world.z());
578  return isCellValid(grid.x(), grid.y(), grid.z());
579 }
580 
581 } // namespace distance_field
582 #endif
double getLocationFromCell(Dimension dim, int cell) const
Gets the center of the cell in world coordinates along the given dimension. No validity check...
Definition: voxel_grid.h:536
void gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const
Converts grid coordinates to world coordinates.
Definition: voxel_grid.h:548
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.
Definition: voxel_grid.h:368
double origin_[3]
The origin (minumum point) of each dimension in meters (in Dimension order)
Definition: voxel_grid.h:296
int stride1_
The step to take when stepping between consecutive X members in the 1D array.
Definition: voxel_grid.h:300
double getOrigin(Dimension dim) const
Gets the origin (miniumum point) of the indicated dimension.
Definition: voxel_grid.h:451
double origin_minus_[3]
origin - 0.5/resolution
Definition: voxel_grid.h:297
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...
Definition: voxel_grid.h:463
double getSize(Dimension dim) const
Gets the size in arbitrary units of the indicated dimension.
Definition: voxel_grid.h:433
static const double origin_y
VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplie...
Definition: voxel_grid.h:62
double size_[3]
The size of each dimension in meters (in Dimension order)
Definition: voxel_grid.h:293
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...
Definition: voxel_grid.h:504
int stride2_
The step to take when stepping between consecutive Y members given an X in the 1D array...
Definition: voxel_grid.h:301
int getCellFromLocation(Dimension dim, double loc) const
Gets the cell number from the location.
Definition: voxel_grid.h:516
bool isCellValid(int x, int y, int z) const
Checks if the given cell in integer coordinates is within the voxel grid.
Definition: voxel_grid.h:409
double y
int num_cells_[3]
The number of cells in each dimension (in Dimension order)
Definition: voxel_grid.h:298
static const double resolution
T * data_
Storage for the full set of data elements.
Definition: voxel_grid.h:290
T *** data_ptrs_
3D array of pointers to the data elements
Definition: voxel_grid.h:292
static const double origin_z
double resolution_
The resolution of each dimension in meters (in Dimension order)
Definition: voxel_grid.h:294
double z
T default_object_
The default object to return in case of out-of-bounds query.
Definition: voxel_grid.h:291
int getNumCells(Dimension dim) const
Gets the number of cells in the indicated dimension.
Definition: voxel_grid.h:457
double oo_resolution_
1.0/resolution_
Definition: voxel_grid.h:295
double getResolution() const
Gets the resolution in arbitrary consistent units.
Definition: voxel_grid.h:439
Namespace for holding classes that generate distance fields.
Dimension
Specifies dimension of different axes.
Definition: voxel_grid.h:48
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 ...
Definition: voxel_grid.h:564
int num_cells_total_
The total number of voxels in the grid.
Definition: voxel_grid.h:299
void reset(const T &initial)
Sets every cell in the voxel grid to the supplied data.
Definition: voxel_grid.h:542
VoxelGrid()
Default constructor for the VoxelGrid.
Definition: voxel_grid.h:351
double x
int ref(int x, int y, int z) const
Gets the 1D index into the array, with no validity check.
Definition: voxel_grid.h:427
T & getCell(int x, int y, int z)
Gives the value of the given location (x,y,z) in the discretized voxel grid space.
Definition: voxel_grid.h:480
static const double origin_x


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33