voxel_grid.cpp
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 #include <voxel_grid/voxel_grid.h>
38 #include <sys/time.h>
39 #include <ros/console.h>
40 
41 namespace voxel_grid {
42  VoxelGrid::VoxelGrid(unsigned int size_x, unsigned int size_y, unsigned int size_z)
43  {
44  size_x_ = size_x;
45  size_y_ = size_y;
46  size_z_ = size_z;
47 
48  if(size_z_ > 16){
49  ROS_INFO("Error, this implementation can only support up to 16 z values (%d)", size_z_);
50  size_z_ = 16;
51  }
52 
53  data_ = new uint32_t[size_x_ * size_y_];
54  uint32_t unknown_col = ~((uint32_t)0)>>16;
55  uint32_t* col = data_;
56  for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
57  *col = unknown_col;
58  ++col;
59  }
60  }
61 
62  void VoxelGrid::resize(unsigned int size_x, unsigned int size_y, unsigned int size_z)
63  {
64  //if we're not actually changing the size, we can just reset things
65  if(size_x == size_x_ && size_y == size_y_ && size_z == size_z_){
66  reset();
67  return;
68  }
69 
70  delete[] data_;
71  size_x_ = size_x;
72  size_y_ = size_y;
73  size_z_ = size_z;
74 
75  if(size_z_ > 16){
76  ROS_INFO("Error, this implementation can only support up to 16 z values (%d)", size_z);
77  size_z_ = 16;
78  }
79 
80  data_ = new uint32_t[size_x_ * size_y_];
81  uint32_t unknown_col = ~((uint32_t)0)>>16;
82  uint32_t* col = data_;
83  for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
84  *col = unknown_col;
85  ++col;
86  }
87  }
88 
90  {
91  delete [] data_;
92  }
93 
95  uint32_t unknown_col = ~((uint32_t)0)>>16;
96  uint32_t* col = data_;
97  for(unsigned int i = 0; i < size_x_ * size_y_; ++i){
98  *col = unknown_col;
99  ++col;
100  }
101  }
102 
103  void VoxelGrid::markVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length){
104  if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
105  ROS_DEBUG("Error, line endpoint out of bounds. (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f), size: (%d, %d, %d)", x0, y0, z0, x1, y1, z1,
107  return;
108  }
109 
110  MarkVoxel mv(data_);
111  raytraceLine(mv, x0, y0, z0, x1, y1, z1, max_length);
112  }
113 
114  void VoxelGrid::clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length){
115  if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
116  ROS_DEBUG("Error, line endpoint out of bounds. (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f), size: (%d, %d, %d)", x0, y0, z0, x1, y1, z1,
118  return;
119  }
120 
121  ClearVoxel cv(data_);
122  raytraceLine(cv, x0, y0, z0, x1, y1, z1, max_length);
123  }
124 
125  void VoxelGrid::clearVoxelLineInMap(double x0, double y0, double z0, double x1, double y1, double z1, unsigned char *map_2d,
126  unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost, unsigned char unknown_cost, unsigned int max_length){
127  costmap = map_2d;
128  if(map_2d == NULL){
129  clearVoxelLine(x0, y0, z0, x1, y1, z1, max_length);
130  return;
131  }
132 
133  if(x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1>=size_x_ || y1>=size_y_ || z1>=size_z_){
134  ROS_DEBUG("Error, line endpoint out of bounds. (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f), size: (%d, %d, %d)", x0, y0, z0, x1, y1, z1,
136  return;
137  }
138 
139  ClearVoxelInMap cvm(data_, costmap, unknown_threshold, mark_threshold, free_cost, unknown_cost);
140  raytraceLine(cvm, x0, y0, z0, x1, y1, z1, max_length);
141  }
142 
143  VoxelStatus VoxelGrid::getVoxel(unsigned int x, unsigned int y, unsigned int z)
144  {
145  if(x >= size_x_ || y >= size_y_ || z >= size_z_){
146  ROS_DEBUG("Error, voxel out of bounds. (%d, %d, %d)\n", x, y, z);
147  return UNKNOWN;
148  }
149  uint32_t full_mask = ((uint32_t)1<<z<<16) | (1<<z);
150  uint32_t result = data_[y * size_x_ + x] & full_mask;
151  unsigned int bits = numBits(result);
152 
153  // known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
154  if(bits < 2){
155  if(bits < 1)
156  return FREE;
157 
158  return UNKNOWN;
159  }
160 
161  return MARKED;
162  }
163 
164  VoxelStatus VoxelGrid::getVoxelColumn(unsigned int x, unsigned int y, unsigned int unknown_threshold, unsigned int marked_threshold)
165  {
166  if(x >= size_x_ || y >= size_y_){
167  ROS_DEBUG("Error, voxel out of bounds. (%d, %d)\n", x, y);
168  return UNKNOWN;
169  }
170 
171  uint32_t* col = &data_[y * size_x_ + x];
172 
173  unsigned int unknown_bits = uint16_t(*col>>16) ^ uint16_t(*col);
174  unsigned int marked_bits = *col>>16;
175 
176  //check if the number of marked bits qualifies the col as marked
177  if(!bitsBelowThreshold(marked_bits, marked_threshold)){
178  return MARKED;
179  }
180 
181  //check if the number of unkown bits qualifies the col as unknown
182  if(!bitsBelowThreshold(unknown_bits, unknown_threshold))
183  return UNKNOWN;
184 
185  return FREE;
186  }
187 
188  unsigned int VoxelGrid::sizeX(){
189  return size_x_;
190  }
191 
192  unsigned int VoxelGrid::sizeY(){
193  return size_y_;
194  }
195 
196  unsigned int VoxelGrid::sizeZ(){
197  return size_z_;
198  }
199 
201  for(unsigned int z = 0; z < size_z_; z++){
202  printf("Layer z = %u:\n",z);
203  for(unsigned int y = 0; y < size_y_; y++){
204  for(unsigned int x = 0 ; x < size_x_; x++){
205  printf((getVoxel(x, y, z)) == voxel_grid::MARKED? "#" : " ");
206  }
207  printf("|\n");
208  }
209  }
210  }
211 
213  printf("Column view:\n");
214  for(unsigned int y = 0; y < size_y_; y++){
215  for(unsigned int x = 0 ; x < size_x_; x++){
216  printf((getVoxelColumn(x, y, 16, 0) == voxel_grid::MARKED)? "#" : " ");
217  }
218  printf("|\n");
219  }
220  }
221 };
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
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
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
#define ROS_INFO(...)
unsigned int size_x_
Definition: voxel_grid.h:320
unsigned int sizeX()
Definition: voxel_grid.cpp:188
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
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 Sun Mar 3 2019 03:44:15