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 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 
00035 /* Author: Mrinal Kalakrishnan, Acorn Pooley */
00036 
00037 #ifndef MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
00038 #define MOVEIT_DISTANCE_FIELD_VOXEL_GRID_
00039 
00040 #include <algorithm>
00041 #include <cmath>
00042 #include <Eigen/Core>
00043 #include <moveit/macros/declare_ptr.h>
00044 
00045 namespace distance_field
00046 {
00048 enum Dimension
00049 {
00050   DIM_X = 0,
00051   DIM_Y = 1,
00052   DIM_Z = 2
00053 };
00054 
00061 template <typename T>
00062 class VoxelGrid
00063 {
00064 public:
00065   MOVEIT_DECLARE_PTR_MEMBER(VoxelGrid);
00066 
00092   VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
00093             double origin_z, T default_object);
00094   virtual ~VoxelGrid();
00095 
00102   VoxelGrid();
00103 
00121   void resize(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
00122               double origin_z, T default_object);
00123 
00137   const T& operator()(double x, double y, double z) const;
00138   const T& operator()(const Eigen::Vector3d& pos) const;
00139 
00157   T& getCell(int x, int y, int z);
00158   T& getCell(const Eigen::Vector3i& pos);
00159   const T& getCell(int x, int y, int z) const;
00160   const T& getCell(const Eigen::Vector3i& pos) const;
00161 
00180   void setCell(int x, int y, int z, const T& obj);
00181   void setCell(const Eigen::Vector3i& pos, const T& obj);
00182 
00188   void reset(const T& initial);
00189 
00197   double getSize(Dimension dim) const;
00198 
00204   double getResolution() const;
00205 
00207   double getResolution(Dimension dim) const;
00208 
00216   double getOrigin(Dimension dim) const;
00217 
00225   int getNumCells(Dimension dim) const;
00226 
00245   void gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const;
00246   void gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3i& world) const;
00247 
00264   bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const;
00265   bool worldToGrid(const Eigen::Vector3i& world, Eigen::Vector3i& grid) const;
00266 
00276   bool isCellValid(int x, int y, int z) const;
00277   bool isCellValid(const Eigen::Vector3i& pos) const;
00278 
00287   bool isCellValid(Dimension dim, int cell) const;
00288 
00289 protected:
00290   T* data_;                
00291   T default_object_;       
00292   T*** data_ptrs_;         
00293   double size_[3];         
00294   double resolution_;      
00295   double oo_resolution_;   
00296   double origin_[3];       
00297   double origin_minus_[3]; 
00298   int num_cells_[3];       
00299   int num_cells_total_;    
00300   int stride1_;            
00301   int stride2_; 
00312   int ref(int x, int y, int z) const;
00313 
00326   int getCellFromLocation(Dimension dim, double loc) const;
00327 
00337   double getLocationFromCell(Dimension dim, int cell) const;
00338 };
00339 
00341 
00342 template <typename T>
00343 VoxelGrid<T>::VoxelGrid(double size_x, double size_y, double size_z, double resolution, double origin_x,
00344                         double origin_y, double origin_z, T default_object)
00345   : data_(NULL)
00346 {
00347   resize(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, default_object);
00348 }
00349 
00350 template <typename T>
00351 VoxelGrid<T>::VoxelGrid() : data_(NULL)
00352 {
00353   for (int i = DIM_X; i <= DIM_Z; ++i)
00354   {
00355     size_[i] = 0;
00356     origin_[i] = 0;
00357     origin_minus_[i] = 0;
00358     num_cells_[i] = 0;
00359   }
00360   resolution_ = 1.0;
00361   oo_resolution_ = 1.0 / resolution_;
00362   num_cells_total_ = 0;
00363   stride1_ = 0;
00364   stride2_ = 0;
00365 }
00366 
00367 template <typename T>
00368 void VoxelGrid<T>::resize(double size_x, double size_y, double size_z, double resolution, double origin_x,
00369                           double origin_y, double origin_z, T default_object)
00370 {
00371   delete[] data_;
00372   data_ = NULL;
00373 
00374   size_[DIM_X] = size_x;
00375   size_[DIM_Y] = size_y;
00376   size_[DIM_Z] = size_z;
00377   origin_[DIM_X] = origin_x;
00378   origin_[DIM_Y] = origin_y;
00379   origin_[DIM_Z] = origin_z;
00380   origin_minus_[DIM_X] = origin_x - 0.5 * resolution;
00381   origin_minus_[DIM_Y] = origin_y - 0.5 * resolution;
00382   origin_minus_[DIM_Z] = origin_z - 0.5 * resolution;
00383   num_cells_total_ = 1;
00384   resolution_ = resolution;
00385   oo_resolution_ = 1.0 / resolution_;
00386   for (int i = DIM_X; i <= DIM_Z; ++i)
00387   {
00388     num_cells_[i] = size_[i] * oo_resolution_;
00389     num_cells_total_ *= num_cells_[i];
00390   }
00391 
00392   default_object_ = default_object;
00393 
00394   stride1_ = num_cells_[DIM_Y] * num_cells_[DIM_Z];
00395   stride2_ = num_cells_[DIM_Z];
00396 
00397   // initialize the data:
00398   if (num_cells_total_ > 0)
00399     data_ = new T[num_cells_total_];
00400 }
00401 
00402 template <typename T>
00403 VoxelGrid<T>::~VoxelGrid()
00404 {
00405   delete[] data_;
00406 }
00407 
00408 template <typename T>
00409 inline bool VoxelGrid<T>::isCellValid(int x, int y, int z) const
00410 {
00411   return (x >= 0 && x < num_cells_[DIM_X] && y >= 0 && y < num_cells_[DIM_Y] && z >= 0 && z < num_cells_[DIM_Z]);
00412 }
00413 
00414 template <typename T>
00415 inline bool VoxelGrid<T>::isCellValid(const Eigen::Vector3i& pos) const
00416 {
00417   return isCellValid(pos.x(), pos.y(), pos.z());
00418 }
00419 
00420 template <typename T>
00421 inline bool VoxelGrid<T>::isCellValid(Dimension dim, int cell) const
00422 {
00423   return cell >= 0 && cell < num_cells_[dim];
00424 }
00425 
00426 template <typename T>
00427 inline int VoxelGrid<T>::ref(int x, int y, int z) const
00428 {
00429   return x * stride1_ + y * stride2_ + z;
00430 }
00431 
00432 template <typename T>
00433 inline double VoxelGrid<T>::getSize(Dimension dim) const
00434 {
00435   return size_[dim];
00436 }
00437 
00438 template <typename T>
00439 inline double VoxelGrid<T>::getResolution() const
00440 {
00441   return resolution_;
00442 }
00443 
00444 template <typename T>
00445 inline double VoxelGrid<T>::getResolution(Dimension dim) const
00446 {
00447   return resolution_;
00448 }
00449 
00450 template <typename T>
00451 inline double VoxelGrid<T>::getOrigin(Dimension dim) const
00452 {
00453   return origin_[dim];
00454 }
00455 
00456 template <typename T>
00457 inline int VoxelGrid<T>::getNumCells(Dimension dim) const
00458 {
00459   return num_cells_[dim];
00460 }
00461 
00462 template <typename T>
00463 inline const T& VoxelGrid<T>::operator()(double x, double y, double z) const
00464 {
00465   int cellX = getCellFromLocation(DIM_X, x);
00466   int cellY = getCellFromLocation(DIM_Y, y);
00467   int cellZ = getCellFromLocation(DIM_Z, z);
00468   if (!isCellValid(cellX, cellY, cellZ))
00469     return default_object_;
00470   return getCell(cellX, cellY, cellZ);
00471 }
00472 
00473 template <typename T>
00474 inline const T& VoxelGrid<T>::operator()(const Eigen::Vector3d& pos) const
00475 {
00476   return this->operator()(pos.x(), pos.y(), pos.z());
00477 }
00478 
00479 template <typename T>
00480 inline T& VoxelGrid<T>::getCell(int x, int y, int z)
00481 {
00482   return data_[ref(x, y, z)];
00483 }
00484 
00485 template <typename T>
00486 inline const T& VoxelGrid<T>::getCell(int x, int y, int z) const
00487 {
00488   return data_[ref(x, y, z)];
00489 }
00490 
00491 template <typename T>
00492 inline T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos)
00493 {
00494   return data_[ref(pos.x(), pos.y(), pos.z())];
00495 }
00496 
00497 template <typename T>
00498 inline const T& VoxelGrid<T>::getCell(const Eigen::Vector3i& pos) const
00499 {
00500   return data_[ref(pos.x(), pos.y(), pos.z())];
00501 }
00502 
00503 template <typename T>
00504 inline void VoxelGrid<T>::setCell(int x, int y, int z, const T& obj)
00505 {
00506   data_[ref(x, y, z)] = obj;
00507 }
00508 
00509 template <typename T>
00510 inline void VoxelGrid<T>::setCell(const Eigen::Vector3i& pos, const T& obj)
00511 {
00512   data_[ref(pos.x(), pos.y(), pos.z())] = obj;
00513 }
00514 
00515 template <typename T>
00516 inline int VoxelGrid<T>::getCellFromLocation(Dimension dim, double loc) const
00517 {
00518   // This implements
00519   //
00520   //      (  loc - origin         )
00521   // floor(  ------------  + 0.5  )
00522   //      (   resolution          )
00523   //
00524   // In other words, the rounded quantized location.
00525   //
00526   // For speed implemented like this:
00527   //
00528   // floor( ( loc - origin_minus ) * oo_resolution )
00529   //
00530   // where  origin_minus = origin - 0.5*resolution
00531   //
00532   return int(floor((loc - origin_minus_[dim]) * oo_resolution_));
00533 }
00534 
00535 template <typename T>
00536 inline double VoxelGrid<T>::getLocationFromCell(Dimension dim, int cell) const
00537 {
00538   return origin_[dim] + resolution_ * (double(cell));
00539 }
00540 
00541 template <typename T>
00542 inline void VoxelGrid<T>::reset(const T& initial)
00543 {
00544   std::fill(data_, data_ + num_cells_total_, initial);
00545 }
00546 
00547 template <typename T>
00548 inline void VoxelGrid<T>::gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const
00549 {
00550   world_x = getLocationFromCell(DIM_X, x);
00551   world_y = getLocationFromCell(DIM_Y, y);
00552   world_z = getLocationFromCell(DIM_Z, z);
00553 }
00554 
00555 template <typename T>
00556 inline void VoxelGrid<T>::gridToWorld(const Eigen::Vector3i& grid, Eigen::Vector3i& world) const
00557 {
00558   world.x() = getLocationFromCell(DIM_X, grid.x());
00559   world.y() = getLocationFromCell(DIM_Y, grid.y());
00560   world.z() = getLocationFromCell(DIM_Z, grid.z());
00561 }
00562 
00563 template <typename T>
00564 inline bool VoxelGrid<T>::worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const
00565 {
00566   x = getCellFromLocation(DIM_X, world_x);
00567   y = getCellFromLocation(DIM_Y, world_y);
00568   z = getCellFromLocation(DIM_Z, world_z);
00569   return isCellValid(x, y, z);
00570 }
00571 
00572 template <typename T>
00573 inline bool VoxelGrid<T>::worldToGrid(const Eigen::Vector3i& world, Eigen::Vector3i& grid) const
00574 {
00575   grid.x() = getCellFromLocation(DIM_X, world.x());
00576   grid.y() = getCellFromLocation(DIM_Y, world.y());
00577   grid.z() = getCellFromLocation(DIM_Z, world.z());
00578   return isCellValid(grid.x(), grid.y(), grid.z());
00579 }
00580 
00581 }  // namespace distance_field
00582 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44