voxel_grid.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Mrinal Kalakrishnan, Acorn Pooley */
 
#pragma once
 
#include <algorithm>
#include <cmath>
#include <Eigen/Core>
#include <moveit/macros/declare_ptr.h>
 
namespace distance_field
{
enum Dimension
{
  DIM_X = 0,
  DIM_Y = 1,
  DIM_Z = 2
};
 
template <typename T>
class VoxelGrid
{
public:
  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);
  virtual ~VoxelGrid();
 
  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);
 
  const T& operator()(double x, double y, double z) const;
  const T& operator()(const Eigen::Vector3d& pos) const;
 
  T& getCell(int x, int y, int z);
  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);
  void setCell(const Eigen::Vector3i& pos, const T& obj);
 
  void reset(const T& initial);
 
  double getSize(Dimension dim) const;
 
  double getResolution() const;
 
  double getResolution(Dimension dim) const;
 
  double getOrigin(Dimension dim) const;
 
  int getNumCells(Dimension dim) const;
 
  void gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
  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;
  bool worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const;
 
  bool isCellValid(int x, int y, int z) const;
  bool isCellValid(const Eigen::Vector3i& pos) const;
 
  bool isCellValid(Dimension dim, int cell) const;
 
protected:
  T* data_;                
  T default_object_;       
  T*** data_ptrs_;         
  double size_[3];         
  double resolution_;      
  double oo_resolution_;   
  double origin_[3];       
  double origin_minus_[3]; 
  int num_cells_[3];       
  int num_cells_total_;    
  int stride1_;            
  int stride2_; 
  int ref(int x, int y, int z) const;
 
  int getCellFromLocation(Dimension dim, double loc) const;
 
  double getLocationFromCell(Dimension dim, int cell) const;
};
 
 
template <typename T>
VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x,
                        double origin_y, double origin_z, T default_object)
  : data_(nullptr)
{
  resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
}
 
template <typename T>
VoxelGrid<T>::VoxelGrid() : data_(nullptr)
{
  for (int i = DIM_X; i <= DIM_Z; ++i)
  {
    size_[i] = 0;
    origin_[i] = 0;
    origin_minus_[i] = 0;
    num_cells_[i] = 0;
  }
  resolution_ = 1.0;
  oo_resolution_ = 1.0 / resolution_;
  num_cells_total_ = 0;
  stride1_ = 0;
  stride2_ = 0;
}
 
template <typename T>
void VoxelGrid<T>::resize(double size_x, double size_y, double size_z, double resolution, double origin_x,
                          double origin_y, double origin_z, T default_object)
{
  delete[] data_;
  data_ = nullptr;
 
  size_[DIM_X] = size_x;
  size_[DIM_Y] = size_y;
  size_[DIM_Z] = size_z;
  origin_[DIM_X] = origin_x;
  origin_[DIM_Y] = origin_y;
  origin_[DIM_Z] = origin_z;
  origin_minus_[DIM_X] = origin_x - 0.5 * resolution;
  origin_minus_[DIM_Y] = origin_y - 0.5 * resolution;
  origin_minus_[DIM_Z] = origin_z - 0.5 * resolution;
  num_cells_total_ = 1;
  resolution_ = resolution;
  oo_resolution_ = 1.0 / resolution_;
  for (int i = DIM_X; i <= DIM_Z; ++i)
  {
    num_cells_[i] = size_[i] * oo_resolution_;
    num_cells_total_ *= num_cells_[i];
  }
 
  default_object_ = default_object;
 
  stride1_ = num_cells_[DIM_Y] * num_cells_[DIM_Z];
  stride2_ = num_cells_[DIM_Z];
 
  // initialize the data:
  if (num_cells_total_ > 0)
    data_ = new T[num_cells_total_];
}
 
template <typename T>
VoxelGrid<T>::~VoxelGrid()
{
  delete[] data_;
}
 
template <typename T>
inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
{
  return (x >= 0 && x < num_cells_[DIM_X] && y >= 0 && y < num_cells_[DIM_Y] && z >= 0 && z < num_cells_[DIM_Z]);
}
 
template <typename T>
inline bool VoxelGrid<T>::isCellValid(const Eigen::Vector3i& pos) const
{
  return isCellValid(pos.x(), pos.y(), pos.z());
}
 
template <typename T>
inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
{
  return cell >= 0 && cell < num_cells_[dim];
}
 
template <typename T>
inline int VoxelGrid<T>::ref(int x, int y, int z) const
{
  return x * stride1_ + y * stride2_ + z;
}
 
template <typename T>
inline double VoxelGrid<T>::getSize(Dimension dim) const
{
  return size_[dim];
}
 
template <typename T>
inline double VoxelGrid<T>::getResolution() const
{
  return resolution_;
}
 
template <typename T>
inline double VoxelGrid<T>::getResolution(Dimension /*dim*/) const
{
  return resolution_;
}
 
template <typename T>
inline double VoxelGrid<T>::getOrigin(Dimension dim) const
{
  return origin_[dim];
}
 
template <typename T>
inline int VoxelGrid<T>::getNumCells(Dimension dim) const
{
  return num_cells_[dim];
}
 
template <typename T>
inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
{
  int cell_x = getCellFromLocation(DIM_X, x);
  int cell_y = getCellFromLocation(DIM_Y, y);
  int cell_z = getCellFromLocation(DIM_Z, z);
  if (!isCellValid(cell_x, cell_y, cell_z))
    return default_object_;
  return getCell(cell_x, cell_y, cell_z);
}
 
template <typename T>
inline const T& VoxelGrid<T>::operator()(const Eigen::Vector3d& pos) const
{
  return this->operator()(pos.x(), pos.y(), pos.z());
}
 
template <typename T>
inline T& VoxelGrid<T>::getCell(int x, int y, int z)
{
  return data_[ref(x, y, z)];
}
 
template <typename T>
inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
{
  return data_[ref(x, y, z)];
}
 
template <typename T>
inline T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos)
{
  return data_[ref(pos.x(), pos.y(), pos.z())];
}
 
template <typename T>
inline const T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos) const
{
  return data_[ref(pos.x(), pos.y(), pos.z())];
}
 
template <typename T>
inline void VoxelGrid<T>::setCell(int x, int y, int z, const T& obj)
{
  data_[ref(x, y, z)] = obj;
}
 
template <typename T>
inline void VoxelGrid<T>::setCell(const Eigen::Vector3i& pos, const T& obj)
{
  data_[ref(pos.x(), pos.y(), pos.z())] = obj;
}
 
template <typename T>
inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
{
  // This implements
  //
  //      (  loc - origin         )
  // floor(  ------------  + 0.5  )
  //      (   resolution          )
  //
  // In other words, the rounded quantized location.
  //
  // For speed implemented like this:
  //
  // floor( ( loc - origin_minus ) * oo_resolution )
  //
  // where  origin_minus = origin - 0.5*resolution
  //
  return int(floor((loc - origin_minus_[dim]) * oo_resolution_));
}
 
template <typename T>
inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
{
  return origin_[dim] + resolution_ * (double(cell));
}
 
template <typename T>
inline void VoxelGrid<T>::reset(const T& initial)
{
  std::fill(data_, data_ + num_cells_total_, initial);
}
 
template <typename T>
inline void VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
{
  world_x = getLocationFromCell(DIM_X, x);
  world_y = getLocationFromCell(DIM_Y, y);
  world_z = getLocationFromCell(DIM_Z, z);
}
 
template <typename T>
inline void VoxelGrid<T>::gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3d& world) const
{
  world.x() = getLocationFromCell(DIM_X, grid.x());
  world.y() = getLocationFromCell(DIM_Y, grid.y());
  world.z() = getLocationFromCell(DIM_Z, grid.z());
}
 
template <typename T>
inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
{
  x = getCellFromLocation(DIM_X, world_x);
  y = getCellFromLocation(DIM_Y, world_y);
  z = getCellFromLocation(DIM_Z, world_z);
  return isCellValid(x, y, z);
}
 
template <typename T>
inline bool VoxelGrid<T>::worldToGrid(const Eigen::Vector3d& world, Eigen::Vector3i& grid) const
{
  grid.x() = getCellFromLocation(DIM_X, world.x());
  grid.y() = getCellFromLocation(DIM_Y, world.y());
  grid.z() = getCellFromLocation(DIM_Z, world.z());
  return isCellValid(grid.x(), grid.y(), grid.z());
}
 
}  // namespace distance_field