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_
00038 #define VOXEL_GRID_VOXEL_GRID_
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 
00050 
00055 namespace voxel_grid {
00056   enum VoxelStatus {
00057     FREE = 0,
00058     UNKNOWN = 1,
00059     MARKED = 2,
00060   };
00061 
00062   class VoxelGrid{
00063     public:
00070       VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z);
00071 
00072       ~VoxelGrid();
00073 
00080       void resize(unsigned int size_x, unsigned int size_y, unsigned int size_z);
00081 
00082       void reset();
00083       uint32_t* getData() {return data_;}
00084 
00085       inline void markVoxel(unsigned int x, unsigned int y, unsigned int z){
00086         if(x >= size_x_ || y >= size_y_ || z >= size_z_){
00087           ROS_DEBUG("Error, voxel out of bounds.\n");
00088           return;
00089         }
00090         uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00091         data_[y * size_x_ + x] |= full_mask; //clear unknown and mark cell
00092       }
00093 
00094       inline bool markVoxelInMap(unsigned int x, unsigned int y, unsigned int z, unsigned int marked_threshold){
00095         if(x >= size_x_ || y >= size_y_ || z >= size_z_){
00096           ROS_DEBUG("Error, voxel out of bounds.\n");
00097           return false;
00098         }
00099 
00100         int index = y * size_x_ + x;
00101         uint32_t* col = &data_[index];
00102         uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00103         *col |= full_mask; //clear unknown and mark cell
00104 
00105         unsigned int marked_bits = *col>>16;
00106 
00107         //make sure the number of bits in each is below our thesholds
00108         return !bitsBelowThreshold(marked_bits, marked_threshold);
00109       }
00110 
00111       inline void clearVoxel(unsigned int x, unsigned int y, unsigned int z){
00112         if(x >= size_x_ || y >= size_y_ || z >= size_z_){
00113           ROS_DEBUG("Error, voxel out of bounds.\n");
00114           return;
00115         }
00116         uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00117         data_[y * size_x_ + x] &= ~(full_mask); //clear unknown and clear cell
00118       }
00119 
00120       inline void clearVoxelColumn(unsigned int index){
00121         ROS_ASSERT(index < size_x_ * size_y_);
00122         data_[index] = 0;
00123       }
00124 
00125       inline void clearVoxelInMap(unsigned int x, unsigned int y, unsigned int z){
00126         if(x >= size_x_ || y >= size_y_ || z >= size_z_){
00127           ROS_DEBUG("Error, voxel out of bounds.\n");
00128           return;
00129         }
00130         int index = y * size_x_ + x;
00131         uint32_t* col = &data_[index];
00132         uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00133         *col &= ~(full_mask); //clear unknown and clear cell
00134 
00135         unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00136         unsigned int marked_bits = *col>>16;
00137 
00138         //make sure the number of bits in each is below our thesholds
00139         if(bitsBelowThreshold(unknown_bits, 1) && bitsBelowThreshold(marked_bits, 1))
00140           costmap[index] = 0;
00141       }
00142 
00143       inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold){
00144         unsigned int bit_count;
00145         for(bit_count = 0; n;){
00146           ++bit_count;
00147           if(bit_count > bit_threshold)
00148             return false;
00149           n &= n - 1; //clear the least significant bit set
00150         }
00151         return true;
00152       }
00153 
00154       static inline unsigned int numBits(unsigned int n){
00155         unsigned int bit_count;
00156         for(bit_count = 0; n; ++bit_count){
00157           n &= n - 1; //clear the least significant bit set
00158         }
00159         return bit_count;
00160 
00161       }
00162 
00163       static VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z,
00164           unsigned int size_x, unsigned int size_y, unsigned int size_z, const uint32_t* data)
00165       {
00166         if(x >= size_x || y >= size_y || z >= size_z){
00167           ROS_DEBUG("Error, voxel out of bounds. (%d, %d, %d)\n", x, y, z);
00168           return UNKNOWN;
00169         }
00170         uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
00171         uint32_t result = data[y * size_x + x] & full_mask;
00172         unsigned int bits = numBits(result);
00173 
00174         // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
00175         if(bits < 2){
00176           if(bits < 1)
00177             return FREE;
00178 
00179           return UNKNOWN;
00180         }
00181 
00182         return MARKED;
00183       }
00184 
00185       void markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length = UINT_MAX);
00186       void clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length = UINT_MAX);
00187       void clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d,
00188           unsigned int unknown_threshold, unsigned int mark_threshold, 
00189           unsigned char free_cost = 0, unsigned char unknown_cost = 255, unsigned int max_length = UINT_MAX);
00190 
00191       VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z);
00192       VoxelStatus getVoxelColumn(unsigned int x, unsigned int y,
00193           unsigned int unknown_threshold = 0, unsigned int marked_threshold = 0); //Are there any obstacles at that (x, y) location in the grid?
00194 
00195       void printVoxelGrid();
00196       void printColumnGrid();
00197       unsigned int sizeX();
00198       unsigned int sizeY();
00199       unsigned int sizeZ();
00200 
00201       template <class ActionType>
00202         inline void raytraceLine(ActionType at, double x0, double y0, double z0,
00203             double x1, double y1, double z1, unsigned int max_length = UINT_MAX){
00204 
00205           int dx = int(x1) - int(x0);
00206           int dy = int(y1) - int(y0);
00207           int dz = int(z1) - int(z0);
00208 
00209           unsigned int abs_dx = abs(dx);
00210           unsigned int abs_dy = abs(dy);
00211           unsigned int abs_dz = abs(dz);
00212 
00213           int offset_dx = sign(dx);
00214           int offset_dy = sign(dy) * size_x_;
00215           int offset_dz = sign(dz);
00216 
00217           unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)z0;
00218           unsigned int offset = (unsigned int)y0 * size_x_ + (unsigned int)x0;
00219 
00220           GridOffset grid_off(offset);
00221           ZOffset z_off(z_mask);
00222 
00223           //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
00224           double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1));
00225           double scale = std::min(1.0,  max_length / dist);
00226 
00227 
00228           //is x dominant
00229           if(abs_dx >= max(abs_dy, abs_dz)){
00230             int error_y = abs_dx / 2;
00231             int error_z = abs_dx / 2;
00232 
00233             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));
00234             return;
00235           }
00236 
00237           //y is dominant
00238           if(abs_dy >= abs_dz){
00239             int error_x = abs_dy / 2;
00240             int error_z = abs_dy / 2;
00241 
00242             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));
00243             return;
00244           }
00245 
00246           //otherwise, z is dominant
00247           int error_x = abs_dz / 2;
00248           int error_y = abs_dz / 2;
00249 
00250           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));
00251         }
00252 
00253     private:
00254 
00255       //the real work is done here... 3D bresenham implementation
00256       template <class ActionType, class OffA, class OffB, class OffC>
00257         inline void bresenham3D(ActionType at, OffA off_a, OffB off_b, OffC off_c,
00258             unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc,
00259             int error_b, int error_c, int offset_a, int offset_b, int offset_c, unsigned int &offset,
00260             unsigned int &z_mask, unsigned int max_length = UINT_MAX){
00261           unsigned int end = std::min(max_length, abs_da);
00262           for(unsigned int i = 0; i < end; ++i){
00263             at(offset, z_mask);
00264             off_a(offset_a);
00265             error_b += abs_db;
00266             error_c += abs_dc;
00267             if((unsigned int)error_b >= abs_da){
00268               off_b(offset_b);
00269               error_b -= abs_da;
00270             }
00271             if((unsigned int)error_c >= abs_da){
00272               off_c(offset_c);
00273               error_c -= abs_da;
00274             }
00275           }
00276           at(offset, z_mask);
00277         }
00278 
00279       inline int sign(int i){
00280         return i > 0 ? 1 : -1;
00281       }
00282 
00283       inline unsigned int max(unsigned int x, unsigned int y){
00284         return x > y ? x : y;
00285       }
00286 
00287       unsigned int size_x_, size_y_, size_z_;
00288       uint32_t *data_;
00289       unsigned char *costmap;
00290 
00291       //Aren't functors so much fun... used to recreate the Bresenham macro Eric wrote in the original version, but in "proper" c++
00292       class MarkVoxel {
00293         public:
00294           MarkVoxel(uint32_t* data): data_(data){}
00295           inline void operator()(unsigned int offset, unsigned int z_mask){
00296             data_[offset] |= z_mask; //clear unknown and mark cell
00297           }
00298         private:
00299           uint32_t* data_;
00300       };
00301 
00302       class ClearVoxel {
00303         public:
00304           ClearVoxel(uint32_t* data): data_(data){}
00305           inline void operator()(unsigned int offset, unsigned int z_mask){
00306             data_[offset] &= ~(z_mask); //clear unknown and clear cell
00307           }
00308         private:
00309           uint32_t* data_;
00310       };
00311 
00312       class ClearVoxelInMap {
00313         public:
00314           ClearVoxelInMap(uint32_t* data, unsigned char *costmap,
00315               unsigned int unknown_clear_threshold, unsigned int marked_clear_threshold, 
00316               unsigned char free_cost = 0, unsigned char unknown_cost = 255): data_(data), costmap_(costmap),
00317         unknown_clear_threshold_(unknown_clear_threshold), marked_clear_threshold_(marked_clear_threshold), 
00318         free_cost_(free_cost), unknown_cost_(unknown_cost){}
00319           inline void operator()(unsigned int offset, unsigned int z_mask){
00320             uint32_t* col = &data_[offset];
00321             *col &= ~(z_mask); //clear unknown and clear cell
00322 
00323             unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00324             unsigned int marked_bits = *col>>16;
00325 
00326             //make sure the number of bits in each is below our thesholds
00327             if(bitsBelowThreshold(marked_bits, marked_clear_threshold_)){
00328               if(bitsBelowThreshold(unknown_bits, unknown_clear_threshold_))
00329                 costmap_[offset] = free_cost_;
00330               else
00331                 costmap_[offset] = unknown_cost_;
00332             }
00333           }
00334         private:
00335           inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold){
00336             unsigned int bit_count;
00337             for(bit_count = 0; n;){
00338               ++bit_count;
00339               if(bit_count > bit_threshold)
00340                 return false;
00341               n &= n - 1; //clear the least significant bit set
00342             }
00343             return true;
00344           }
00345           uint32_t* data_;
00346           unsigned char *costmap_;
00347           unsigned int unknown_clear_threshold_, marked_clear_threshold_;
00348           unsigned char free_cost_, unknown_cost_;
00349       };
00350 
00351       class GridOffset {
00352         public:
00353           GridOffset(unsigned int &offset) : offset_(offset) {}
00354           inline void operator()(int offset_val){
00355             offset_ += offset_val;
00356           }
00357         private:
00358           unsigned int &offset_;
00359       };
00360 
00361       class ZOffset {
00362         public:
00363           ZOffset(unsigned int &z_mask) : z_mask_(z_mask) {}
00364           inline void operator()(int offset_val){
00365             offset_val > 0 ? z_mask_ <<= 1 : z_mask_ >>= 1;
00366           }
00367         private:
00368           unsigned int & z_mask_;
00369       };
00370 
00371   };
00372 };
00373 
00374 #endif


voxel_grid
Author(s): Eitan Marder-Eppstein, Eric Berger
autogenerated on Mon Oct 6 2014 02:44:13