00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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;
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;
00104
00105 unsigned int marked_bits = *col>>16;
00106
00107
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);
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);
00134
00135 unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00136 unsigned int marked_bits = *col>>16;
00137
00138
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;
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;
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
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);
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
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
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
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
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
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
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;
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);
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);
00322
00323 unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00324 unsigned int marked_bits = *col>>16;
00325
00326
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;
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