Point Cloud Library (PCL) 1.15.1
Loading...
Searching...
No Matches
voxel_structure.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 *
37 */
38
39
40#ifndef PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_
41#define PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_
42
43
44namespace pcl
45{
46
47namespace recognition
48{
49
50template<class T, typename REAL> inline void
51VoxelStructure<T,REAL>::build (const REAL bounds[6], int num_of_voxels[3])
52{
53 this->clear();
54
55 // Copy the bounds
56 bounds_[0] = bounds[0];
57 bounds_[1] = bounds[1];
58 bounds_[2] = bounds[2];
59 bounds_[3] = bounds[3];
60 bounds_[4] = bounds[4];
61 bounds_[5] = bounds[5];
62
63 num_of_voxels_[0] = num_of_voxels[0];
64 num_of_voxels_[1] = num_of_voxels[1];
65 num_of_voxels_[2] = num_of_voxels[2];
66 num_of_voxels_xy_plane_ = num_of_voxels[0]*num_of_voxels[1];
67 total_num_of_voxels_ = num_of_voxels_xy_plane_*num_of_voxels[2];
68
69 // Allocate memory for the voxels
70 voxels_ = new T[total_num_of_voxels_];
71
72 // Compute the spacing between the voxels in x, y and z direction
73 spacing_[0] = (bounds[1]-bounds[0])/static_cast<REAL> (num_of_voxels[0]);
74 spacing_[1] = (bounds[3]-bounds[2])/static_cast<REAL> (num_of_voxels[1]);
75 spacing_[2] = (bounds[5]-bounds[4])/static_cast<REAL> (num_of_voxels[2]);
76
77 // Compute the center of the voxel with integer coordinates (0, 0, 0)
78 min_center_[0] = bounds_[0] + static_cast<REAL> (0.5)*spacing_[0];
79 min_center_[1] = bounds_[2] + static_cast<REAL> (0.5)*spacing_[1];
80 min_center_[2] = bounds_[4] + static_cast<REAL> (0.5)*spacing_[2];
81}
82
83
84template<class T, typename REAL> inline T*
85VoxelStructure<T,REAL>::getVoxel (const REAL p[3])
86{
87 if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] )
88 return nullptr;
89
90 int x = static_cast<int> ((p[0] - bounds_[0])/spacing_[0]);
91 int y = static_cast<int> ((p[1] - bounds_[2])/spacing_[1]);
92 int z = static_cast<int> ((p[2] - bounds_[4])/spacing_[2]);
93
94 return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x];
95}
96
97
98template<class T, typename REAL> inline T*
99VoxelStructure<T,REAL>::getVoxel (int x, int y, int z) const
100{
101 if ( x < 0 || x >= num_of_voxels_[0] ) return nullptr;
102 if ( y < 0 || y >= num_of_voxels_[1] ) return nullptr;
103 if ( z < 0 || z >= num_of_voxels_[2] ) return nullptr;
104
105 return &voxels_[z*num_of_voxels_xy_plane_ + y*num_of_voxels_[0] + x];
106}
107
108
109template<class T, typename REAL> inline int
110VoxelStructure<T,REAL>::getNeighbors (const REAL* p, T **neighs) const
111{
112 if ( p[0] < bounds_[0] || p[0] >= bounds_[1] || p[1] < bounds_[2] || p[1] >= bounds_[3] || p[2] < bounds_[4] || p[2] >= bounds_[5] )
113 return 0;
114
115 const int x = static_cast<int> ((p[0] - bounds_[0])/spacing_[0]);
116 const int y = static_cast<int> ((p[1] - bounds_[2])/spacing_[1]);
117 const int z = static_cast<int> ((p[2] - bounds_[4])/spacing_[2]);
118
119 const int x_m1 = x-1, x_p1 = x+1;
120 const int y_m1 = y-1, y_p1 = y+1;
121 const int z_m1 = z-1, z_p1 = z+1;
122
123 T* voxel;
124 int num_neighs = 0;
125
126 voxel = this->getVoxel (x_p1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
127 voxel = this->getVoxel (x_p1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel;
128 voxel = this->getVoxel (x_p1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
129 voxel = this->getVoxel (x_p1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
130 voxel = this->getVoxel (x_p1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel;
131 voxel = this->getVoxel (x_p1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
132 voxel = this->getVoxel (x_p1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
133 voxel = this->getVoxel (x_p1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel;
134 voxel = this->getVoxel (x_p1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
135
136 voxel = this->getVoxel (x , y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
137 voxel = this->getVoxel (x , y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel;
138 voxel = this->getVoxel (x , y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
139 voxel = this->getVoxel (x , y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
140 voxel = this->getVoxel (x , y , z ); if ( voxel ) neighs[num_neighs++] = voxel;
141 voxel = this->getVoxel (x , y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
142 voxel = this->getVoxel (x , y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
143 voxel = this->getVoxel (x , y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel;
144 voxel = this->getVoxel (x , y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
145
146 voxel = this->getVoxel (x_m1, y_p1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
147 voxel = this->getVoxel (x_m1, y_p1, z ); if ( voxel ) neighs[num_neighs++] = voxel;
148 voxel = this->getVoxel (x_m1, y_p1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
149 voxel = this->getVoxel (x_m1, y , z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
150 voxel = this->getVoxel (x_m1, y , z ); if ( voxel ) neighs[num_neighs++] = voxel;
151 voxel = this->getVoxel (x_m1, y , z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
152 voxel = this->getVoxel (x_m1, y_m1, z_p1); if ( voxel ) neighs[num_neighs++] = voxel;
153 voxel = this->getVoxel (x_m1, y_m1, z ); if ( voxel ) neighs[num_neighs++] = voxel;
154 voxel = this->getVoxel (x_m1, y_m1, z_m1); if ( voxel ) neighs[num_neighs++] = voxel;
155
156 return num_neighs;
157}
158
159} // namespace recognition
160} // namespace pcl
161
162#endif // PCL_RECOGNITION_VOXEL_STRUCTURE_HPP_
163
pcl::recognition::VoxelStructure::build
void build(const REAL bounds[6], int num_of_voxels[3])
Call this method before using an instance of this class.
Definition voxel_structure.hpp:51
pcl::recognition::VoxelStructure::getVoxel
T * getVoxel(const REAL p[3])
Returns a pointer to the voxel which contains p or NULL if p is not inside the structure.
Definition voxel_structure.hpp:85
pcl::recognition::VoxelStructure::clear
void clear()
Release the memory allocated for the voxels.
Definition voxel_structure.h:59
pcl::recognition::VoxelStructure::getNeighbors
int getNeighbors(const REAL *p, T **neighs) const
Saves pointers to the voxels which are neighbors of the voxels which contains 'p'.
Definition voxel_structure.hpp:110