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


voxel_grid
Author(s): Eitan Marder-Eppstein, Eric Berger, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:15