voxel_grid.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: Eitan Marder-Eppstein
00036  *********************************************************************/
00037 #ifndef VOXEL_GRID_VOXEL_GRID_H
00038 #define VOXEL_GRID_VOXEL_GRID_H
00039 
00040 #include <stdio.h>
00041 #include <string.h>
00042 #include <stdlib.h>
00043 #include <stdint.h>
00044 #include <math.h>
00045 #include <limits.h>
00046 #include <algorithm>
00047 #include <ros/console.h>
00048 #include <ros/assert.h>
00049 
00056 namespace voxel_grid
00057 {
00058 
00059 enum VoxelStatus {
00060   FREE = 0,
00061   UNKNOWN = 1,
00062   MARKED = 2,
00063 };
00064 
00065 class VoxelGrid
00066 {
00067 public:
00074   VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z);
00075 
00076   ~VoxelGrid();
00077 
00084   void resize(unsigned int size_x, unsigned int size_y, unsigned int size_z);
00085 
00086   void reset();
00087   uint32_t* getData() { return data_; }
00088 
00089   inline void markVoxel(unsigned int x, unsigned int y, unsigned int z)
00090   {
00091     if (x >= size_x_ || y >= size_y_ || z >= size_z_)
00092     {
00093       ROS_DEBUG("Error, voxel out of bounds.\n");
00094       return;
00095     }
00096     uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00097     data_[y * size_x_ + x] |= full_mask; //clear unknown and mark cell
00098   }
00099 
00100   inline bool markVoxelInMap(unsigned int x, unsigned int y, unsigned int z, unsigned int marked_threshold)
00101   {
00102     if (x >= size_x_ || y >= size_y_ || z >= size_z_)
00103     {
00104       ROS_DEBUG("Error, voxel out of bounds.\n");
00105       return false;
00106     }
00107 
00108     int index = y * size_x_ + x;
00109     uint32_t* col = &data_[index];
00110     uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00111     *col |= full_mask; //clear unknown and mark cell
00112 
00113     unsigned int marked_bits = *col>>16;
00114 
00115     //make sure the number of bits in each is below our thesholds
00116     return !bitsBelowThreshold(marked_bits, marked_threshold);
00117   }
00118 
00119   inline void clearVoxel(unsigned int x, unsigned int y, unsigned int z)
00120   {
00121     if (x >= size_x_ || y >= size_y_ || z >= size_z_)
00122     {
00123       ROS_DEBUG("Error, voxel out of bounds.\n");
00124       return;
00125     }
00126     uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00127     data_[y * size_x_ + x] &= ~(full_mask); //clear unknown and clear cell
00128   }
00129 
00130   inline void clearVoxelColumn(unsigned int index)
00131   {
00132     ROS_ASSERT(index < size_x_ * size_y_);
00133     data_[index] = 0;
00134   }
00135 
00136   inline void clearVoxelInMap(unsigned int x, unsigned int y, unsigned int z)
00137   {
00138     if(x >= size_x_ || y >= size_y_ || z >= size_z_)
00139     {
00140       ROS_DEBUG("Error, voxel out of bounds.\n");
00141       return;
00142     }
00143     int index = y * size_x_ + x;
00144     uint32_t* col = &data_[index];
00145     uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00146     *col &= ~(full_mask); //clear unknown and clear cell
00147 
00148     unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00149     unsigned int marked_bits = *col>>16;
00150 
00151     //make sure the number of bits in each is below our thesholds
00152     if (bitsBelowThreshold(unknown_bits, 1) && bitsBelowThreshold(marked_bits, 1))
00153     {
00154       costmap[index] = 0;
00155     }
00156   }
00157 
00158   inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
00159   {
00160     unsigned int bit_count;
00161     for (bit_count = 0; n;)
00162     {
00163       ++bit_count;
00164       if (bit_count > bit_threshold)
00165       {
00166         return false;
00167       }
00168       n &= n - 1; //clear the least significant bit set
00169     }
00170     return true;
00171   }
00172 
00173   static inline unsigned int numBits(unsigned int n)
00174   {
00175     unsigned int bit_count;
00176     for (bit_count = 0; n; ++bit_count)
00177     {
00178       n &= n - 1; //clear the least significant bit set
00179     }
00180     return bit_count;
00181   }
00182 
00183   static VoxelStatus getVoxel(
00184     unsigned int x, unsigned int y, unsigned int z,
00185     unsigned int size_x, unsigned int size_y, unsigned int size_z, const uint32_t* data)
00186   {
00187     if (x >= size_x || y >= size_y || z >= size_z)
00188     {
00189       ROS_DEBUG("Error, voxel out of bounds. (%d, %d, %d)\n", x, y, z);
00190       return UNKNOWN;
00191     }
00192     uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00193     uint32_t result = data[y * size_x + x] & full_mask;
00194     unsigned int bits = numBits(result);
00195 
00196     // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
00197     if (bits < 2)
00198     {
00199       if (bits < 1)
00200       {
00201         return FREE;
00202       }
00203       return UNKNOWN;
00204     }
00205     return MARKED;
00206   }
00207 
00208   void markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length = UINT_MAX);
00209   void clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length = UINT_MAX);
00210   void clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d,
00211                            unsigned int unknown_threshold, unsigned int mark_threshold,
00212                            unsigned char free_cost = 0, unsigned char unknown_cost = 255, unsigned int max_length = UINT_MAX);
00213 
00214   VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z);
00215 
00216   //Are there any obstacles at that (x, y) location in the grid?
00217   VoxelStatus getVoxelColumn(unsigned int x, unsigned int y,
00218                              unsigned int unknown_threshold = 0, unsigned int marked_threshold = 0);
00219 
00220   void printVoxelGrid();
00221   void printColumnGrid();
00222   unsigned int sizeX();
00223   unsigned int sizeY();
00224   unsigned int sizeZ();
00225 
00226   template <class ActionType>
00227   inline void raytraceLine(
00228     ActionType at, double x0, double y0, double z0,
00229     double x1, double y1, double z1, unsigned int max_length = UINT_MAX)
00230   {
00231     int dx = int(x1) - int(x0);
00232     int dy = int(y1) - int(y0);
00233     int dz = int(z1) - int(z0);
00234 
00235     unsigned int abs_dx = abs(dx);
00236     unsigned int abs_dy = abs(dy);
00237     unsigned int abs_dz = abs(dz);
00238 
00239     int offset_dx = sign(dx);
00240     int offset_dy = sign(dy) * size_x_;
00241     int offset_dz = sign(dz);
00242 
00243     unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)z0;
00244     unsigned int offset = (unsigned int)y0 * size_x_ + (unsigned int)x0;
00245 
00246     GridOffset grid_off(offset);
00247     ZOffset z_off(z_mask);
00248 
00249     //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
00250     double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1));
00251     double scale = std::min(1.0,  max_length / dist);
00252 
00253     //is x dominant
00254     if (abs_dx >= max(abs_dy, abs_dz))
00255     {
00256       int error_y = abs_dx / 2;
00257       int error_z = abs_dx / 2;
00258 
00259       bresenham3D(at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z, offset_dx, offset_dy, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dx));
00260       return;
00261     }
00262 
00263     //y is dominant
00264     if (abs_dy >= abs_dz)
00265     {
00266       int error_x = abs_dy / 2;
00267       int error_z = abs_dy / 2;
00268 
00269       bresenham3D(at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z, offset_dy, offset_dx, offset_dz, offset, z_mask, (unsigned int)(scale * abs_dy));
00270       return;
00271     }
00272 
00273     //otherwise, z is dominant
00274     int error_x = abs_dz / 2;
00275     int error_y = abs_dz / 2;
00276 
00277     bresenham3D(at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz, offset_dx, offset_dy, offset, z_mask, (unsigned int)(scale * abs_dz));
00278   }
00279 
00280 private:
00281   //the real work is done here... 3D bresenham implementation
00282   template <class ActionType, class OffA, class OffB, class OffC>
00283   inline void bresenham3D(
00284     ActionType at, OffA off_a, OffB off_b, OffC off_c,
00285     unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc,
00286     int error_b, int error_c, int offset_a, int offset_b, int offset_c, unsigned int &offset,
00287     unsigned int &z_mask, unsigned int max_length = UINT_MAX)
00288   {
00289     unsigned int end = std::min(max_length, abs_da);
00290     for (unsigned int i = 0; i < end; ++i)
00291     {
00292       at(offset, z_mask);
00293       off_a(offset_a);
00294       error_b += abs_db;
00295       error_c += abs_dc;
00296       if ((unsigned int)error_b >= abs_da)
00297       {
00298         off_b(offset_b);
00299         error_b -= abs_da;
00300       }
00301       if ((unsigned int)error_c >= abs_da)
00302       {
00303         off_c(offset_c);
00304         error_c -= abs_da;
00305       }
00306     }
00307     at(offset, z_mask);
00308   }
00309 
00310   inline int sign(int i)
00311   {
00312     return i > 0 ? 1 : -1;
00313   }
00314 
00315   inline unsigned int max(unsigned int x, unsigned int y)
00316   {
00317     return x > y ? x : y;
00318   }
00319 
00320   unsigned int size_x_, size_y_, size_z_;
00321   uint32_t *data_;
00322   unsigned char *costmap;
00323 
00324   //Aren't functors so much fun... used to recreate the Bresenham macro Eric wrote in the original version, but in "proper" c++
00325   class MarkVoxel
00326   {
00327   public:
00328     MarkVoxel(uint32_t* data): data_(data){}
00329     inline void operator()(unsigned int offset, unsigned int z_mask)
00330     {
00331       data_[offset] |= z_mask; //clear unknown and mark cell
00332     }
00333   private:
00334     uint32_t* data_;
00335   };
00336 
00337   class ClearVoxel
00338   {
00339   public:
00340     ClearVoxel(uint32_t* data): data_(data){}
00341     inline void operator()(unsigned int offset, unsigned int z_mask)
00342     {
00343       data_[offset] &= ~(z_mask); //clear unknown and clear cell
00344     }
00345   private:
00346     uint32_t* data_;
00347   };
00348 
00349   class ClearVoxelInMap
00350   {
00351   public:
00352     ClearVoxelInMap(
00353       uint32_t* data, unsigned char *costmap,
00354       unsigned int unknown_clear_threshold, unsigned int marked_clear_threshold,
00355       unsigned char free_cost = 0, unsigned char unknown_cost = 255): data_(data), costmap_(costmap),
00356       unknown_clear_threshold_(unknown_clear_threshold), marked_clear_threshold_(marked_clear_threshold),
00357       free_cost_(free_cost), unknown_cost_(unknown_cost)
00358     {
00359     }
00360 
00361     inline void operator()(unsigned int offset, unsigned int z_mask)
00362     {
00363       uint32_t* col = &data_[offset];
00364       *col &= ~(z_mask); //clear unknown and clear cell
00365 
00366       unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00367       unsigned int marked_bits = *col>>16;
00368 
00369       //make sure the number of bits in each is below our thesholds
00370       if (bitsBelowThreshold(marked_bits, marked_clear_threshold_))
00371       {
00372         if (bitsBelowThreshold(unknown_bits, unknown_clear_threshold_))
00373         {
00374           costmap_[offset] = free_cost_;
00375         }
00376         else
00377         {
00378           costmap_[offset] = unknown_cost_;
00379         }
00380       }
00381     }
00382   private:
00383     inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
00384     {
00385       unsigned int bit_count;
00386       for (bit_count = 0; n;)
00387       {
00388         ++bit_count;
00389         if (bit_count > bit_threshold)
00390         {
00391           return false;
00392         }
00393         n &= n - 1; //clear the least significant bit set
00394       }
00395       return true;
00396     }
00397 
00398     uint32_t* data_;
00399     unsigned char *costmap_;
00400     unsigned int unknown_clear_threshold_, marked_clear_threshold_;
00401     unsigned char free_cost_, unknown_cost_;
00402   };
00403 
00404   class GridOffset
00405   {
00406   public:
00407     GridOffset(unsigned int &offset) : offset_(offset) {}
00408     inline void operator()(int offset_val)
00409     {
00410       offset_ += offset_val;
00411     }
00412   private:
00413     unsigned int &offset_;
00414   };
00415 
00416   class ZOffset
00417   {
00418   public:
00419     ZOffset(unsigned int &z_mask) : z_mask_(z_mask) {}
00420     inline void operator()(int offset_val)
00421     {
00422       offset_val > 0 ? z_mask_ <<= 1 : z_mask_ >>= 1;
00423     }
00424   private:
00425     unsigned int & z_mask_;
00426   };
00427 };
00428 
00429 }  // namespace voxel_grid
00430 
00431 #endif  // VOXEL_GRID_VOXEL_GRID_H


voxel_grid
Author(s): Eitan Marder-Eppstein, Eric Berger, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:06:47