voxel_grid.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Eitan Marder-Eppstein
36  *********************************************************************/
37 #ifndef VOXEL_GRID_VOXEL_GRID_H
38 #define VOXEL_GRID_VOXEL_GRID_H
39 
40 #include <stdio.h>
41 #include <string.h>
42 #include <stdlib.h>
43 #include <stdint.h>
44 #include <math.h>
45 #include <limits.h>
46 #include <algorithm>
47 #include <ros/console.h>
48 #include <ros/assert.h>
49 
56 namespace voxel_grid
57 {
58 
60  FREE = 0,
61  UNKNOWN = 1,
62  MARKED = 2,
63 };
64 
65 class VoxelGrid
66 {
67 public:
74  VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z);
75 
76  ~VoxelGrid();
77 
84  void resize(unsigned int size_x, unsigned int size_y, unsigned int size_z);
85 
86  void reset();
87  uint32_t* getData() { return data_; }
88 
89  inline void markVoxel(unsigned int x, unsigned int y, unsigned int z)
90  {
91  if (x >= size_x_ || y >= size_y_ || z >= size_z_)
92  {
93  ROS_DEBUG("Error, voxel out of bounds.\n");
94  return;
95  }
96  uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
97  data_[y * size_x_ + x] |= full_mask; //clear unknown and mark cell
98  }
99 
100  inline bool markVoxelInMap(unsigned int x, unsigned int y, unsigned int z, unsigned int marked_threshold)
101  {
102  if (x >= size_x_ || y >= size_y_ || z >= size_z_)
103  {
104  ROS_DEBUG("Error, voxel out of bounds.\n");
105  return false;
106  }
107 
108  int index = y * size_x_ + x;
109  uint32_t* col = &data_[index];
110  uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
111  *col |= full_mask; //clear unknown and mark cell
112 
113  unsigned int marked_bits = *col>>16;
114 
115  //make sure the number of bits in each is below our thesholds
116  return !bitsBelowThreshold(marked_bits, marked_threshold);
117  }
118 
119  inline void clearVoxel(unsigned int x, unsigned int y, unsigned int z)
120  {
121  if (x >= size_x_ || y >= size_y_ || z >= size_z_)
122  {
123  ROS_DEBUG("Error, voxel out of bounds.\n");
124  return;
125  }
126  uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
127  data_[y * size_x_ + x] &= ~(full_mask); //clear unknown and clear cell
128  }
129 
130  inline void clearVoxelColumn(unsigned int index)
131  {
132  ROS_ASSERT(index < size_x_ * size_y_);
133  data_[index] = 0;
134  }
135 
136  inline void clearVoxelInMap(unsigned int x, unsigned int y, unsigned int z)
137  {
138  if(x >= size_x_ || y >= size_y_ || z >= size_z_)
139  {
140  ROS_DEBUG("Error, voxel out of bounds.\n");
141  return;
142  }
143  int index = y * size_x_ + x;
144  uint32_t* col = &data_[index];
145  uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
146  *col &= ~(full_mask); //clear unknown and clear cell
147 
148  unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
149  unsigned int marked_bits = *col>>16;
150 
151  //make sure the number of bits in each is below our thesholds
152  if (bitsBelowThreshold(unknown_bits, 1) && bitsBelowThreshold(marked_bits, 1))
153  {
154  costmap[index] = 0;
155  }
156  }
157 
158  inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
159  {
160  unsigned int bit_count;
161  for (bit_count = 0; n;)
162  {
163  ++bit_count;
164  if (bit_count > bit_threshold)
165  {
166  return false;
167  }
168  n &= n - 1; //clear the least significant bit set
169  }
170  return true;
171  }
172 
173  static inline unsigned int numBits(unsigned int n)
174  {
175  unsigned int bit_count;
176  for (bit_count = 0; n; ++bit_count)
177  {
178  n &= n - 1; //clear the least significant bit set
179  }
180  return bit_count;
181  }
182 
184  unsigned int x, unsigned int y, unsigned int z,
185  unsigned int size_x, unsigned int size_y, unsigned int size_z, const uint32_t* data)
186  {
187  if (x >= size_x || y >= size_y || z >= size_z)
188  {
189  ROS_DEBUG("Error, voxel out of bounds. (%d, %d, %d)\n", x, y, z);
190  return UNKNOWN;
191  }
192  uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
193  uint32_t result = data[y * size_x + x] & full_mask;
194  unsigned int bits = numBits(result);
195 
196  // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
197  if (bits < 2)
198  {
199  if (bits < 1)
200  {
201  return FREE;
202  }
203  return UNKNOWN;
204  }
205  return MARKED;
206  }
207 
208  void markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length = UINT_MAX);
209  void clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length = UINT_MAX);
210  void clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d,
211  unsigned int unknown_threshold, unsigned int mark_threshold,
212  unsigned char free_cost = 0, unsigned char unknown_cost = 255, unsigned int max_length = UINT_MAX);
213 
214  VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z);
215 
216  //Are there any obstacles at that (x, y) location in the grid?
217  VoxelStatus getVoxelColumn(unsigned int x, unsigned int y,
218  unsigned int unknown_threshold = 0, unsigned int marked_threshold = 0);
219 
220  void printVoxelGrid();
221  void printColumnGrid();
222  unsigned int sizeX();
223  unsigned int sizeY();
224  unsigned int sizeZ();
225 
226  template <class ActionType>
227  inline void raytraceLine(
228  ActionType at, double x0, double y0, double z0,
229  double x1, double y1, double z1, unsigned int max_length = UINT_MAX)
230  {
231  int dx = int(x1) - int(x0);
232  int dy = int(y1) - int(y0);
233  int dz = int(z1) - int(z0);
234 
235  unsigned int abs_dx = abs(dx);
236  unsigned int abs_dy = abs(dy);
237  unsigned int abs_dz = abs(dz);
238 
239  int offset_dx = sign(dx);
240  int offset_dy = sign(dy) * size_x_;
241  int offset_dz = sign(dz);
242 
243  unsigned int z_mask = ((1 << 16) | 1) << (unsigned int)z0;
244  unsigned int offset = (unsigned int)y0 * size_x_ + (unsigned int)x0;
245 
246  GridOffset grid_off(offset);
247  ZOffset z_off(z_mask);
248 
249  //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
250  double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1) + (z0 - z1) * (z0 - z1));
251  double scale = std::min(1.0, max_length / dist);
252 
253  //is x dominant
254  if (abs_dx >= max(abs_dy, abs_dz))
255  {
256  int error_y = abs_dx / 2;
257  int error_z = abs_dx / 2;
258 
259  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));
260  return;
261  }
262 
263  //y is dominant
264  if (abs_dy >= abs_dz)
265  {
266  int error_x = abs_dy / 2;
267  int error_z = abs_dy / 2;
268 
269  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));
270  return;
271  }
272 
273  //otherwise, z is dominant
274  int error_x = abs_dz / 2;
275  int error_y = abs_dz / 2;
276 
277  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));
278  }
279 
280 private:
281  //the real work is done here... 3D bresenham implementation
282  template <class ActionType, class OffA, class OffB, class OffC>
283  inline void bresenham3D(
284  ActionType at, OffA off_a, OffB off_b, OffC off_c,
285  unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc,
286  int error_b, int error_c, int offset_a, int offset_b, int offset_c, unsigned int &offset,
287  unsigned int &z_mask, unsigned int max_length = UINT_MAX)
288  {
289  unsigned int end = std::min(max_length, abs_da);
290  for (unsigned int i = 0; i < end; ++i)
291  {
292  at(offset, z_mask);
293  off_a(offset_a);
294  error_b += abs_db;
295  error_c += abs_dc;
296  if ((unsigned int)error_b >= abs_da)
297  {
298  off_b(offset_b);
299  error_b -= abs_da;
300  }
301  if ((unsigned int)error_c >= abs_da)
302  {
303  off_c(offset_c);
304  error_c -= abs_da;
305  }
306  }
307  at(offset, z_mask);
308  }
309 
310  inline int sign(int i)
311  {
312  return i > 0 ? 1 : -1;
313  }
314 
315  inline unsigned int max(unsigned int x, unsigned int y)
316  {
317  return x > y ? x : y;
318  }
319 
320  unsigned int size_x_, size_y_, size_z_;
321  uint32_t *data_;
322  unsigned char *costmap;
323 
324  //Aren't functors so much fun... used to recreate the Bresenham macro Eric wrote in the original version, but in "proper" c++
325  class MarkVoxel
326  {
327  public:
328  MarkVoxel(uint32_t* data): data_(data){}
329  inline void operator()(unsigned int offset, unsigned int z_mask)
330  {
331  data_[offset] |= z_mask; //clear unknown and mark cell
332  }
333  private:
334  uint32_t* data_;
335  };
336 
338  {
339  public:
340  ClearVoxel(uint32_t* data): data_(data){}
341  inline void operator()(unsigned int offset, unsigned int z_mask)
342  {
343  data_[offset] &= ~(z_mask); //clear unknown and clear cell
344  }
345  private:
346  uint32_t* data_;
347  };
348 
350  {
351  public:
353  uint32_t* data, unsigned char *costmap,
354  unsigned int unknown_clear_threshold, unsigned int marked_clear_threshold,
355  unsigned char free_cost = 0, unsigned char unknown_cost = 255): data_(data), costmap_(costmap),
356  unknown_clear_threshold_(unknown_clear_threshold), marked_clear_threshold_(marked_clear_threshold),
357  free_cost_(free_cost), unknown_cost_(unknown_cost)
358  {
359  }
360 
361  inline void operator()(unsigned int offset, unsigned int z_mask)
362  {
363  uint32_t* col = &data_[offset];
364  *col &= ~(z_mask); //clear unknown and clear cell
365 
366  unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
367  unsigned int marked_bits = *col>>16;
368 
369  //make sure the number of bits in each is below our thesholds
370  if (bitsBelowThreshold(marked_bits, marked_clear_threshold_))
371  {
372  if (bitsBelowThreshold(unknown_bits, unknown_clear_threshold_))
373  {
374  costmap_[offset] = free_cost_;
375  }
376  else
377  {
378  costmap_[offset] = unknown_cost_;
379  }
380  }
381  }
382  private:
383  inline bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
384  {
385  unsigned int bit_count;
386  for (bit_count = 0; n;)
387  {
388  ++bit_count;
389  if (bit_count > bit_threshold)
390  {
391  return false;
392  }
393  n &= n - 1; //clear the least significant bit set
394  }
395  return true;
396  }
397 
398  uint32_t* data_;
399  unsigned char *costmap_;
400  unsigned int unknown_clear_threshold_, marked_clear_threshold_;
401  unsigned char free_cost_, unknown_cost_;
402  };
403 
405  {
406  public:
407  GridOffset(unsigned int &offset) : offset_(offset) {}
408  inline void operator()(int offset_val)
409  {
410  offset_ += offset_val;
411  }
412  private:
413  unsigned int &offset_;
414  };
415 
416  class ZOffset
417  {
418  public:
419  ZOffset(unsigned int &z_mask) : z_mask_(z_mask) {}
420  inline void operator()(int offset_val)
421  {
422  offset_val > 0 ? z_mask_ <<= 1 : z_mask_ >>= 1;
423  }
424  private:
425  unsigned int & z_mask_;
426  };
427 };
428 
429 } // namespace voxel_grid
430 
431 #endif // VOXEL_GRID_VOXEL_GRID_H
void clearVoxel(unsigned int x, unsigned int y, unsigned int z)
Definition: voxel_grid.h:119
void operator()(unsigned int offset, unsigned int z_mask)
Definition: voxel_grid.h:329
VoxelStatus getVoxelColumn(unsigned int x, unsigned int y, unsigned int unknown_threshold=0, unsigned int marked_threshold=0)
Definition: voxel_grid.cpp:164
static VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z, unsigned int size_x, unsigned int size_y, unsigned int size_z, const uint32_t *data)
Definition: voxel_grid.h:183
ClearVoxelInMap(uint32_t *data, unsigned char *costmap, unsigned int unknown_clear_threshold, unsigned int marked_clear_threshold, unsigned char free_cost=0, unsigned char unknown_cost=255)
Definition: voxel_grid.h:352
void bresenham3D(ActionType at, OffA off_a, OffB off_b, OffC off_c, unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc, int error_b, int error_c, int offset_a, int offset_b, int offset_c, unsigned int &offset, unsigned int &z_mask, unsigned int max_length=UINT_MAX)
Definition: voxel_grid.h:283
ZOffset(unsigned int &z_mask)
Definition: voxel_grid.h:419
void operator()(int offset_val)
Definition: voxel_grid.h:408
unsigned char * costmap
Definition: voxel_grid.h:322
VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z)
Constructor for a voxel grid.
Definition: voxel_grid.cpp:42
void operator()(int offset_val)
Definition: voxel_grid.h:420
void clearVoxelInMap(unsigned int x, unsigned int y, unsigned int z)
Definition: voxel_grid.h:136
bool markVoxelInMap(unsigned int x, unsigned int y, unsigned int z, unsigned int marked_threshold)
Definition: voxel_grid.h:100
GridOffset(unsigned int &offset)
Definition: voxel_grid.h:407
unsigned int size_z_
Definition: voxel_grid.h:320
void clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d, unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost=0, unsigned char unknown_cost=255, unsigned int max_length=UINT_MAX)
Definition: voxel_grid.cpp:125
bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
Definition: voxel_grid.h:158
void resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
Resizes a voxel grid to the desired size.
Definition: voxel_grid.cpp:62
unsigned int sizeY()
Definition: voxel_grid.cpp:192
void operator()(unsigned int offset, unsigned int z_mask)
Definition: voxel_grid.h:361
void operator()(unsigned int offset, unsigned int z_mask)
Definition: voxel_grid.h:341
unsigned int max(unsigned int x, unsigned int y)
Definition: voxel_grid.h:315
unsigned int size_x_
Definition: voxel_grid.h:320
unsigned int sizeX()
Definition: voxel_grid.cpp:188
bool bitsBelowThreshold(unsigned int n, unsigned int bit_threshold)
Definition: voxel_grid.h:383
void markVoxel(unsigned int x, unsigned int y, unsigned int z)
Definition: voxel_grid.h:89
uint32_t * getData()
Definition: voxel_grid.h:87
void markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)
Definition: voxel_grid.cpp:103
static unsigned int numBits(unsigned int n)
Definition: voxel_grid.h:173
unsigned int size_y_
Definition: voxel_grid.h:320
void clearVoxelColumn(unsigned int index)
Definition: voxel_grid.h:130
#define ROS_ASSERT(cond)
unsigned int sizeZ()
Definition: voxel_grid.cpp:196
void raytraceLine(ActionType at, double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)
Definition: voxel_grid.h:227
void clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)
Definition: voxel_grid.cpp:114
#define ROS_DEBUG(...)


voxel_grid
Author(s): Eitan Marder-Eppstein, Eric Berger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:40