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_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;
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;
00112
00113 unsigned int marked_bits = *col>>16;
00114
00115
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);
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);
00147
00148 unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00149 unsigned int marked_bits = *col>>16;
00150
00151
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;
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;
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
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
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
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
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
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
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
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
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;
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);
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);
00365
00366 unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
00367 unsigned int marked_bits = *col>>16;
00368
00369
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;
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 }
00430
00431 #endif // VOXEL_GRID_VOXEL_GRID_H