test_voxel_grid.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan, Ken Anderson */
36 
37 #include <gtest/gtest.h>
38 
40 #include <ros/ros.h>
41 
42 using namespace distance_field;
43 
44 TEST(TestVoxelGrid, TestReadWrite)
45 {
46  int i;
47  int def = -100;
48  VoxelGrid<int> vg(0.02, 0.02, 0.02, 0.01, 0, 0, 0, def);
49 
50  int num_x = vg.getNumCells(DIM_X);
51  int num_y = vg.getNumCells(DIM_Y);
52  int num_z = vg.getNumCells(DIM_Z);
53 
54  // Check dimensions
55  EXPECT_EQ(num_x, 2);
56  EXPECT_EQ(num_y, 2);
57  EXPECT_EQ(num_z, 2);
58 
59  // check initial values
60  vg.reset(0);
61 
62  i = 0;
63  for (int x = 0; x < num_x; x++)
64  for (int y = 0; y < num_y; y++)
65  for (int z = 0; z < num_z; z++)
66  {
67  EXPECT_EQ(vg.getCell(x, y, z), 0);
68  i++;
69  }
70 
71  // Check out-of-bounds query // FIXME-- this test fails!!
72  // EXPECT_EQ( vg.getCell(999,9999,999), def );
73  // EXPECT_EQ( vg.getCell(numX+1,0,0), def);
74  // EXPECT_EQ( vg.getCell(0,numY+1,0), def);
75  // EXPECT_EQ( vg.getCell(0,0,numZ+1), def);
76 
77  // Set values
78  i = 0;
79  for (int x = 0; x < num_x; x++)
80  for (int y = 0; y < num_y; y++)
81  for (int z = 0; z < num_z; z++)
82  {
83  vg.getCell(x, y, z) = i;
84  i++;
85  }
86 
87  // check reset values
88  i = 0;
89  for (int x = 0; x < num_x; x++)
90  for (int y = 0; y < num_y; y++)
91  for (int z = 0; z < num_z; z++)
92  {
93  EXPECT_EQ(i, vg.getCell(x, y, z));
94  i++;
95  }
96 }
97 
98 int main(int argc, char** argv)
99 {
100  testing::InitGoogleTest(&argc, argv);
101  return RUN_ALL_TESTS();
102 }
main
int main(int argc, char **argv)
Definition: test_voxel_grid.cpp:98
distance_field::VoxelGrid::reset
void reset(const T &initial)
Sets every cell in the voxel grid to the supplied data.
Definition: voxel_grid.h:573
ros.h
distance_field::VoxelGrid
VoxelGrid holds a dense 3D, axis-aligned set of data at a given resolution, where the data is supplie...
Definition: voxel_grid.h:93
y
double y
distance_field::DIM_Y
@ DIM_Y
Definition: voxel_grid.h:114
distance_field::DIM_Z
@ DIM_Z
Definition: voxel_grid.h:115
distance_field::DIM_X
@ DIM_X
Definition: voxel_grid.h:113
distance_field
Namespace for holding classes that generate distance fields.
Definition: distance_field.h:64
TEST
TEST(TestVoxelGrid, TestReadWrite)
Definition: test_voxel_grid.cpp:44
voxel_grid.h
x
double x
distance_field::VoxelGrid::getNumCells
int getNumCells(Dimension dim) const
Gets the number of cells in the indicated dimension.
Definition: voxel_grid.h:488
distance_field::VoxelGrid::getCell
T & getCell(int x, int y, int z)
Gives the value of the given location (x,y,z) in the discretized voxel grid space.
Definition: voxel_grid.h:511
z
double z
EXPECT_EQ
#define EXPECT_EQ(a, b)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Nov 21 2024 03:23:42