voxel_grid.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #ifndef DF_VOXEL_GRID_H_
00038 #define DF_VOXEL_GRID_H_
00039 
00040 #include <algorithm>
00041 #include <math.h>
00042 
00043 namespace distance_field
00044 {
00045 
00049 template <typename T>
00050 class VoxelGrid
00051 {
00052 public:
00065   VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00066       double origin_x, double origin_y, double origin_z, T default_object);
00067   virtual ~VoxelGrid();
00068 
00072   const T& operator()(double x, double y, double z) const;
00073 
00077   T& getCell(int x, int y, int z);
00078 
00082   void setCell(int x, int y, int z, T& obj);
00083 
00087   const T& getCell(int x, int y, int z) const;
00088 
00092   void reset(T initial);
00093 
00094   enum Dimension
00095   {
00096     DIM_X = 0,
00097     DIM_Y = 1,
00098     DIM_Z = 2
00099   };
00100 
00104   double getSize(Dimension dim) const;
00105 
00109   double getResolution(Dimension dim) const;
00110 
00114   double getOrigin(Dimension dim) const;
00115 
00119   int getNumCells(Dimension dim) const;
00120 
00124   bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00125 
00129   bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00130 
00131 protected:
00132   T* data_;                     
00133   T default_object_;            
00134   T*** data_ptrs_;
00135   double size_[3];
00136   double resolution_[3];
00137   double origin_[3];
00138   int num_cells_[3];
00139   int num_cells_total_;
00140   int stride1_;
00141   int stride2_;
00142 
00146   int ref(int x, int y, int z) const;
00147 
00151   int getCellFromLocation(Dimension dim, double loc) const;
00152 
00156   double getLocationFromCell(Dimension dim, int cell) const;
00157 
00161   bool isCellValid(int x, int y, int z) const;
00162 
00166   bool isCellValid(Dimension dim, int cell) const;
00167 };
00168 
00170 
00171 template<typename T>
00172 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution,
00173     double origin_x, double origin_y, double origin_z, T default_object)
00174 {
00175   size_[DIM_X] = size_x;
00176   size_[DIM_Y] = size_y;
00177   size_[DIM_Z] = size_z;
00178   origin_[DIM_X] = origin_x;
00179   origin_[DIM_Y] = origin_y;
00180   origin_[DIM_Z] = origin_z;
00181   num_cells_total_ = 1;
00182   for (int i=DIM_X; i<=DIM_Z; ++i)
00183   {
00184     resolution_[i] = resolution;
00185     num_cells_[i] = size_[i] / resolution_[i];
00186     num_cells_total_ *= num_cells_[i];
00187   }
00188   default_object_ = default_object;
00189 
00190   stride1_ = num_cells_[DIM_Y]*num_cells_[DIM_Z];
00191   stride2_ = num_cells_[DIM_Z];
00192 
00193   // initialize the data:
00194   data_ = new T[num_cells_total_];
00195 
00196 }
00197 
00198 template<typename T>
00199 VoxelGrid<T>::~VoxelGrid()
00200 {
00201   delete[] data_;
00202 }
00203 
00204 template<typename T>
00205 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
00206 {
00207   return (
00208       x>=0 && x<num_cells_[DIM_X] &&
00209       y>=0 && y<num_cells_[DIM_Y] &&
00210       z>=0 && z<num_cells_[DIM_Z]);
00211 }
00212 
00213 template<typename T>
00214 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
00215 {
00216   return cell>=0 && cell<num_cells_[dim];
00217 }
00218 
00219 template<typename T>
00220 inline int VoxelGrid<T>::ref(int x, int y, int z) const
00221 {
00222   return x*stride1_ + y*stride2_ + z;
00223 }
00224 
00225 template<typename T>
00226 inline double VoxelGrid<T>::getSize(Dimension dim) const
00227 {
00228   return size_[dim];
00229 }
00230 
00231 template<typename T>
00232 inline double VoxelGrid<T>::getResolution(Dimension dim) const
00233 {
00234   return resolution_[dim];
00235 }
00236 
00237 template<typename T>
00238 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
00239 {
00240   return origin_[dim];
00241 }
00242 
00243 template<typename T>
00244 inline int VoxelGrid<T>::getNumCells(Dimension dim) const
00245 {
00246   return num_cells_[dim];
00247 }
00248 
00249 template<typename T>
00250 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
00251 {
00252   int cellX = getCellFromLocation(DIM_X, x);
00253   int cellY = getCellFromLocation(DIM_Y, y);
00254   int cellZ = getCellFromLocation(DIM_Z, z);
00255   if (!isCellValid(cellX, cellY, cellZ))
00256     return default_object_;
00257   return getCell(cellX, cellY, cellZ);
00258 }
00259 
00260 template<typename T>
00261 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
00262 {
00263   return data_[ref(x,y,z)];
00264 }
00265 
00266 template<typename T>
00267 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
00268 {
00269   return data_[ref(x,y,z)];
00270 }
00271 
00272 template<typename T>
00273 inline void VoxelGrid<T>::setCell(int x, int y, int z, T& obj)
00274 {
00275   data_[ref(x,y,z)] = obj;
00276 }
00277 
00278 template<typename T>
00279 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
00280 {
00281   double res = (loc-origin_[dim])/resolution_[dim];
00282   if (res > 0)
00283     return floor(res + 0.5);
00284   else
00285     return ceil(res - 0.5);
00286 }
00287 
00288 template<typename T>
00289 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
00290 {
00291   return origin_[dim] + resolution_[dim]*(double(cell));
00292 }
00293 
00294 
00295 template<typename T>
00296 inline void VoxelGrid<T>::reset(T initial)
00297 {
00298   std::fill(data_, data_+num_cells_total_, initial);
00299 }
00300 
00301 template<typename T>
00302 inline bool VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
00303 {
00304   world_x = getLocationFromCell(DIM_X, x);
00305   world_y = getLocationFromCell(DIM_Y, y);
00306   world_z = getLocationFromCell(DIM_Z, z);
00307   return true;
00308 }
00309 
00310 template<typename T>
00311 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
00312 {
00313   x = getCellFromLocation(DIM_X, world_x);
00314   y = getCellFromLocation(DIM_Y, world_y);
00315   z = getCellFromLocation(DIM_Z, world_z);
00316   return isCellValid(x,y,z);
00317 }
00318 
00319 } // namespace distance_field
00320 #endif /* DF_VOXEL_GRID_H_ */


distance_field
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:08:36