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 */

#ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
#define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_

#include <algorithm>
#include <cmath>
#include <Eigen/Core>

namespace distance_field
{

enum Dimension {
  DIM_X = 0,
  DIM_Y = 1,
  DIM_Z = 2
};

template <typename T>
class VoxelGrid
{
public:
  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::Vector3i& world) const;

  bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
  bool worldToGrid(const Eigen::Vector3i& 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_(NULL)
{
  resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
}

template<typename T>
VoxelGrid<T>::VoxelGrid()
  : data_(NULL)
{
  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_ = NULL;

  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 cellX = getCellFromLocation(DIM_X, x);
  int cellY = getCellFromLocation(DIM_Y, y);
  int cellZ = getCellFromLocation(DIM_Z, z);
  if (!isCellValid(cellX, cellY, cellZ))
    return default_object_;
  return getCell(cellX, cellY, cellZ);
}

template<typename T>
inline const T& VoxelGrid<T>::operator()(const Eigen::Vector3d& pos) const
{
  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::Vector3i& 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::Vector3i& 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
#endif