moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 #pragma once
38 
39 #include <algorithm>
40 #include <cmath>
41 #include <Eigen/Core>
43 
44 namespace distance_field
45 {
48 {
49  DIM_X = 0,
50  DIM_Y = 1,
51  DIM_Z = 2
52 };
53 
60 template <typename T>
61 class VoxelGrid
62 {
63 public:
65 
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);
93  virtual ~VoxelGrid();
94 
102 
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);
122 
136  const T& operator()(double x, double y, double z) const;
137  const T& operator()(const Eigen::Vector3d& pos) const;
138 
156  T& getCell(int x, int y, int z);
157  T& getCell(const Eigen::Vector3i& pos);
158  const T& getCell(int x, int y, int z) const;
159  const T& getCell(const Eigen::Vector3i& pos) const;
160 
179  void setCell(int x, int y, int z, const T& obj);
180  void setCell(const Eigen::Vector3i& pos, const T& obj);
181 
187  void reset(const T& initial);
188 
196  double getSize(Dimension dim) const;
197 
203  double getResolution() const;
204 
206  double getResolution(Dimension dim) const;
207 
215  double getOrigin(Dimension dim) const;
216 
224  int getNumCells(Dimension dim) const;
225 
244  void gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
245  void gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3d& world) const;
246 
263  bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
264  bool worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const;
265 
275  bool isCellValid(int x, int y, int z) const;
276  bool isCellValid(const Eigen::Vector3i& pos) const;
277 
286  bool isCellValid(Dimension dim, int cell) const;
287 
288 protected:
289  T* data_;
291  T*** data_ptrs_;
292  double size_[3];
293  double resolution_;
294  double oo_resolution_;
295  double origin_[3];
296  double origin_minus_[3];
297  int num_cells_[3];
299  int stride1_;
300  int stride2_;
311  int ref(int x, int y, int z) const;
312 
325  int getCellFromLocation(Dimension dim, double loc) const;
326 
336  double getLocationFromCell(Dimension dim, int cell) const;
337 };
338 
340 
341 template <typename T>
342 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x,
343  double origin_y, double origin_z, T default_object)
344  : data_(nullptr)
345 {
346  resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
347 }
348 
349 template <typename T>
350 VoxelGrid<T>::VoxelGrid() : data_(nullptr)
351 {
352  for (int i = DIM_X; i <= DIM_Z; ++i)
353  {
354  size_[i] = 0;
355  origin_[i] = 0;
356  origin_minus_[i] = 0;
357  num_cells_[i] = 0;
358  }
359  resolution_ = 1.0;
360  oo_resolution_ = 1.0 / resolution_;
361  num_cells_total_ = 0;
362  stride1_ = 0;
363  stride2_ = 0;
364 }
365 
366 template <typename T>
367 void VoxelGrid<T>::resize(double size_x, double size_y, double size_z, double resolution, double origin_x,
368  double origin_y, double origin_z, T default_object)
369 {
370  delete[] data_;
371  data_ = nullptr;
372 
373  size_[DIM_X] = size_x;
374  size_[DIM_Y] = size_y;
375  size_[DIM_Z] = size_z;
376  origin_[DIM_X] = origin_x;
377  origin_[DIM_Y] = origin_y;
378  origin_[DIM_Z] = origin_z;
379  origin_minus_[DIM_X] = origin_x - 0.5 * resolution;
380  origin_minus_[DIM_Y] = origin_y - 0.5 * resolution;
381  origin_minus_[DIM_Z] = origin_z - 0.5 * resolution;
382  num_cells_total_ = 1;
383  resolution_ = resolution;
384  oo_resolution_ = 1.0 / resolution_;
385  for (int i = DIM_X; i <= DIM_Z; ++i)
386  {
387  num_cells_[i] = size_[i] * oo_resolution_;
388  num_cells_total_ *= num_cells_[i];
389  }
390 
391  default_object_ = default_object;
392 
393  stride1_ = num_cells_[DIM_Y] * num_cells_[DIM_Z];
394  stride2_ = num_cells_[DIM_Z];
395 
396  // initialize the data:
397  if (num_cells_total_ > 0)
398  data_ = new T[num_cells_total_];
399 }
400 
401 template <typename T>
403 {
404  delete[] data_;
405 }
406 
407 template <typename T>
408 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
409 {
410  return (x >= 0 && x < num_cells_[DIM_X] && y >= 0 && y < num_cells_[DIM_Y] && z >= 0 && z < num_cells_[DIM_Z]);
411 }
412 
413 template <typename T>
414 inline bool VoxelGrid<T>::isCellValid(const Eigen::Vector3i& pos) const
415 {
416  return isCellValid(pos.x(), pos.y(), pos.z());
417 }
418 
419 template <typename T>
420 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
421 {
422  return cell >= 0 && cell < num_cells_[dim];
423 }
424 
425 template <typename T>
426 inline int VoxelGrid<T>::ref(int x, int y, int z) const
427 {
428  return x * stride1_ + y * stride2_ + z;
429 }
430 
431 template <typename T>
432 inline double VoxelGrid<T>::getSize(Dimension dim) const
433 {
434  return size_[dim];
435 }
436 
437 template <typename T>
438 inline double VoxelGrid<T>::getResolution() const
439 {
440  return resolution_;
441 }
442 
443 template <typename T>
444 inline double VoxelGrid<T>::getResolution(Dimension /*dim*/) const
445 {
446  return resolution_;
447 }
448 
449 template <typename T>
450 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
451 {
452  return origin_[dim];
453 }
454 
455 template <typename T>
457 {
458  return num_cells_[dim];
459 }
460 
461 template <typename T>
462 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
463 {
464  int cell_x = getCellFromLocation(DIM_X, x);
465  int cell_y = getCellFromLocation(DIM_Y, y);
466  int cell_z = getCellFromLocation(DIM_Z, z);
467  if (!isCellValid(cell_x, cell_y, cell_z))
468  return default_object_;
469  return getCell(cell_x, cell_y, cell_z);
470 }
471 
472 template <typename T>
473 inline const T& VoxelGrid<T>::operator()(const Eigen::Vector3d& pos) const
474 {
475  return this->operator()(pos.x(), pos.y(), pos.z());
476 }
477 
478 template <typename T>
479 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
480 {
481  return data_[ref(x, y, z)];
482 }
483 
484 template <typename T>
485 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
486 {
487  return data_[ref(x, y, z)];
488 }
489 
490 template <typename T>
491 inline T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos)
492 {
493  return data_[ref(pos.x(), pos.y(), pos.z())];
494 }
495 
496 template <typename T>
497 inline const T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos) const
498 {
499  return data_[ref(pos.x(), pos.y(), pos.z())];
500 }
501 
502 template <typename T>
503 inline void VoxelGrid<T>::setCell(int x, int y, int z, const T& obj)
504 {
505  data_[ref(x, y, z)] = obj;
506 }
507 
508 template <typename T>
509 inline void VoxelGrid<T>::setCell(const Eigen::Vector3i& pos, const T& obj)
510 {
511  data_[ref(pos.x(), pos.y(), pos.z())] = obj;
512 }
513 
514 template <typename T>
515 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
516 {
517  // This implements
518  //
519  // ( loc - origin )
520  // floor( ------------ + 0.5 )
521  // ( resolution )
522  //
523  // In other words, the rounded quantized location.
524  //
525  // For speed implemented like this:
526  //
527  // floor( ( loc - origin_minus ) * oo_resolution )
528  //
529  // where origin_minus = origin - 0.5*resolution
530  //
531  return int(floor((loc - origin_minus_[dim]) * oo_resolution_));
532 }
533 
534 template <typename T>
535 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
536 {
537  return origin_[dim] + resolution_ * (double(cell));
538 }
539 
540 template <typename T>
541 inline void VoxelGrid<T>::reset(const T& initial)
542 {
543  std::fill(data_, data_ + num_cells_total_, initial);
544 }
545 
546 template <typename T>
547 inline void VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
548 {
549  world_x = getLocationFromCell(DIM_X, x);
550  world_y = getLocationFromCell(DIM_Y, y);
551  world_z = getLocationFromCell(DIM_Z, z);
552 }
553 
554 template <typename T>
555 inline void VoxelGrid<T>::gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3d& world) const
556 {
557  world.x() = getLocationFromCell(DIM_X, grid.x());
558  world.y() = getLocationFromCell(DIM_Y, grid.y());
559  world.z() = getLocationFromCell(DIM_Z, grid.z());
560 }
561 
562 template <typename T>
563 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
564 {
565  x = getCellFromLocation(DIM_X, world_x);
566  y = getCellFromLocation(DIM_Y, world_y);
567  z = getCellFromLocation(DIM_Z, world_z);
568  return isCellValid(x, y, z);
569 }
570 
571 template <typename T>
572 inline bool VoxelGrid<T>::worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const
573 {
574  grid.x() = getCellFromLocation(DIM_X, world.x());
575  grid.y() = getCellFromLocation(DIM_Y, world.y());
576  grid.z() = getCellFromLocation(DIM_Z, world.z());
577  return isCellValid(grid.x(), grid.y(), grid.z());
578 }
579 
580 } // namespace distance_field
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:292
int stride2_
The step to take when stepping between consecutive Y members given an X in the 1D array.
Definition: voxel_grid.h:300
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:563
VoxelGrid()
Default constructor for the VoxelGrid.
Definition: voxel_grid.h:350
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:462
double oo_resolution_
1.0/resolution_
Definition: voxel_grid.h:294
T *** data_ptrs_
3D array of pointers to the data elements
Definition: voxel_grid.h:291
int num_cells_total_
The total number of voxels in the grid.
Definition: voxel_grid.h:298
int getNumCells(Dimension dim) const
Gets the number of cells in the indicated dimension.
Definition: voxel_grid.h:456
int getCellFromLocation(Dimension dim, double loc) const
Gets the cell number from the location.
Definition: voxel_grid.h:515
void setCell(const Eigen::Vector3i &pos, const T &obj)
Definition: voxel_grid.h:509
double origin_minus_[3]
origin - 0.5/resolution
Definition: voxel_grid.h:296
const T & getCell(const Eigen::Vector3i &pos) const
Definition: voxel_grid.h:497
bool isCellValid(const Eigen::Vector3i &pos) const
Definition: voxel_grid.h:414
bool isCellValid(Dimension dim, int cell) const
Checks if the indicated index is valid along a particular dimension.
Definition: voxel_grid.h:420
double origin_[3]
The origin (minimum point) of each dimension in meters (in Dimension order)
Definition: voxel_grid.h:295
int stride1_
The step to take when stepping between consecutive X members in the 1D array.
Definition: voxel_grid.h:299
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:479
double resolution_
The resolution 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:503
void reset(const T &initial)
Sets every cell in the voxel grid to the supplied data.
Definition: voxel_grid.h:541
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:535
MOVEIT_DECLARE_PTR_MEMBER(VoxelGrid)
T & getCell(const Eigen::Vector3i &pos)
Definition: voxel_grid.h:491
const T & operator()(const Eigen::Vector3d &pos) const
Definition: voxel_grid.h:473
double getResolution(Dimension dim) const
deprecated. Use the version with no arguments.
Definition: voxel_grid.h:444
int num_cells_[3]
The number of cells in each dimension (in Dimension order)
Definition: voxel_grid.h:297
double getOrigin(Dimension dim) const
Gets the origin (minimum point) of the indicated dimension.
Definition: voxel_grid.h:450
const T & getCell(int x, int y, int z) const
Definition: voxel_grid.h:485
T * data_
Storage for the full set of data elements.
Definition: voxel_grid.h:289
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:408
T default_object_
The default object to return in case of out-of-bounds query.
Definition: voxel_grid.h:290
bool worldToGrid(const Eigen::Vector3d &world, Eigen::Vector3i &grid) const
Definition: voxel_grid.h:572
double getSize(Dimension dim) const
Gets the size in arbitrary units of the indicated dimension.
Definition: voxel_grid.h:432
void gridToWorld(const Eigen::Vector3i &grid, Eigen::Vector3d &world) const
Definition: voxel_grid.h:555
int ref(int x, int y, int z) const
Gets the 1D index into the array, with no validity check.
Definition: voxel_grid.h:426
double getResolution() const
Gets the resolution in arbitrary consistent units.
Definition: voxel_grid.h:438
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.
Definition: voxel_grid.h:342
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:367
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:547
Namespace for holding classes that generate distance fields.
Dimension
Specifies dimension of different axes.
Definition: voxel_grid.h:48
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
x
Definition: pick.py:64
y
Definition: pick.py:65
z
Definition: pick.py:66