voxel_grid_tests.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, 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 Willow Garage, Inc. 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 <gtest/gtest.h>
39 
40 TEST(voxel_grid, basicMarkingAndClearing){
41  int size_x = 50, size_y = 10, size_z = 16;
42  voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
43 
44  //Put a "tabletop" into the scene. A flat rectangle of set voxels at z = 12.
45  int table_z = 12;
46  int table_x_min = 5, table_x_max = 15;
47  int table_y_min = 0, table_y_max = 3;
48  for(int x = table_x_min; x <= table_x_max; x++){
49  vg.markVoxelLine(x, table_y_min, table_z, x, table_y_max, table_z);
50  }
51 
52  for(int i = table_x_min; i <= table_x_max; ++i){
53  for(int j = table_y_min; j <= table_y_max; ++j){
54  //check that each cell of the table is marked
55  ASSERT_EQ(voxel_grid::MARKED, vg.getVoxel(i, j, table_z));
56  }
57  }
58 
59  int mark_count = 0;
60  unsigned int unknown_count = 0;
61  //go through each cell in the voxel grid and make sure that only 44 are filled in
62  for(unsigned int i = 0; i < vg.sizeX(); ++i){
63  for(unsigned int j = 0; j < vg.sizeY(); ++j){
64  for(unsigned int k = 0; k < vg.sizeZ(); ++k){
65  if(vg.getVoxel(i, j, k) == voxel_grid::MARKED){
66  mark_count++;
67  }
68  else if(vg.getVoxel(i, j, k) == voxel_grid::UNKNOWN){
69  unknown_count++;
70  }
71  }
72  }
73  }
74  ASSERT_EQ(mark_count, 44);
75 
76  //the rest of the cells should be unknown
77  ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);
78 
79  //now, let's clear one of the rows of the table
80  vg.clearVoxelLine(table_x_min, table_y_min, table_z, table_x_max, table_y_min, table_z);
81 
82  mark_count = 0;
83  unknown_count = 0;
84  int free_count = 0;
85  //go through each cell in the voxel grid and make sure that only 33 are now filled in
86  for(unsigned int i = 0; i < vg.sizeX(); ++i){
87  for(unsigned int j = 0; j < vg.sizeY(); ++j){
88  for(unsigned int k = 0; k < vg.sizeZ(); ++k){
89  if(vg.getVoxel(i, j, k) == voxel_grid::MARKED){
90  mark_count++;
91  }
92  else if(vg.getVoxel(i, j, k) == voxel_grid::FREE){
93  free_count++;
94  }
95  else if(vg.getVoxel(i, j, k) == voxel_grid::UNKNOWN){
96  unknown_count++;
97  }
98  }
99  }
100  }
101 
102  //we've switched 11 cells from marked to free
103  ASSERT_EQ(mark_count, 33);
104 
105  //we've just explicitly seen through 11 cells
106  ASSERT_EQ(free_count, 11);
107 
108  //the rest of the cells should still be unknown
109  ASSERT_EQ(unknown_count, vg.sizeX() * vg.sizeY() * vg.sizeZ() - 44);
110 
111  //now let's put in a vertical column manually to test markVoxel
112  for(unsigned int i = 0; i < vg.sizeZ(); ++i){
113  vg.markVoxel(0, 0, i);
114  ASSERT_EQ(vg.getVoxel(0, 0, i), voxel_grid::MARKED);
115  }
116 
117  vg.printColumnGrid();
118  vg.printVoxelGrid();
119 
120  //now, let's clear that line of voxels and make sure that they clear out OK
121  vg.clearVoxelLine(0, 0, 0, 0, 0, vg.sizeZ() - 1);
122 
123  for(unsigned int i = 0; i < vg.sizeZ(); ++i){
124  ASSERT_EQ(vg.getVoxel(0, 0, i), voxel_grid::FREE);
125  }
126 
127  mark_count = 0;
128 
129  //Visualize the output
130  /*
131  v->printVoxelGrid();
132  v->printColumnGrid();
133 
134  printf("CostMap:\n===========\n");
135  for(int y = 0; y < size_y; y++){
136  for(int x = 0; x < size_x; x++){
137  printf((costMap[y * size_x + x] > 0 ? "#" : " "));
138  }printf("|\n");
139  }
140  */
141 }
142 
143 int main(int argc, char** argv){
144  testing::InitGoogleTest(&argc, argv);
145  return RUN_ALL_TESTS();
146 }
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
int main(int argc, char **argv)
unsigned int sizeY()
Definition: voxel_grid.cpp:192
unsigned int sizeX()
Definition: voxel_grid.cpp:188
void markVoxel(unsigned int x, unsigned int y, unsigned int z)
Definition: voxel_grid.h:89
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
unsigned int sizeZ()
Definition: voxel_grid.cpp:196
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
TEST(voxel_grid, basicMarkingAndClearing)


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