Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
voxel_grid_occlusion_estimation.hpp
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, Inc. 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 * Author : Christian Potthast
35 * Email : potthast@usc.edu
36 *
37 */
38
39#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
40#define PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
41
42#include <pcl/filters/voxel_grid_occlusion_estimation.h>
43
44//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45template <typename PointT> void
47{
48 // initialization set to true
49 initialized_ = true;
50
51 // create the voxel grid and store the output cloud
52 this->filter (filtered_cloud_);
53
54 // Get the minimum and maximum bounding box dimensions
55 b_min_[0] = (static_cast<float> ( min_b_[0]) * leaf_size_[0]);
56 b_min_[1] = (static_cast<float> ( min_b_[1]) * leaf_size_[1]);
57 b_min_[2] = (static_cast<float> ( min_b_[2]) * leaf_size_[2]);
58 b_max_[0] = (static_cast<float> ( (max_b_[0]) + 1) * leaf_size_[0]);
59 b_max_[1] = (static_cast<float> ( (max_b_[1]) + 1) * leaf_size_[1]);
60 b_max_[2] = (static_cast<float> ( (max_b_[2]) + 1) * leaf_size_[2]);
61
62 // set the sensor origin and sensor orientation
63 sensor_origin_ = filtered_cloud_.sensor_origin_;
64 sensor_orientation_ = filtered_cloud_.sensor_orientation_;
65}
66
67//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68template <typename PointT> int
70 const Eigen::Vector3i& in_target_voxel)
71{
72 if (!initialized_)
73 {
74 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
75 return -1;
76 }
77
78 // estimate direction to target voxel
79 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
80 Eigen::Vector4f direction = p - sensor_origin_;
81 direction.normalize ();
82
83 // estimate entry point into the voxel grid
84 float tmin = rayBoxIntersection (sensor_origin_, direction);
85
86 if (tmin == -1)
87 {
88 PCL_ERROR ("The ray does not intersect with the bounding box \n");
89 return -1;
90 }
91
92 // ray traversal
93 out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin);
94
95 return 0;
96}
97
98//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
99template <typename PointT> int
101 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
102 const Eigen::Vector3i& in_target_voxel)
103{
104 if (!initialized_)
105 {
106 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
107 return -1;
108 }
109
110 // estimate direction to target voxel
111 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
112 Eigen::Vector4f direction = p - sensor_origin_;
113 direction.normalize ();
114
115 // estimate entry point into the voxel grid
116 float tmin = rayBoxIntersection (sensor_origin_, direction);
117
118 if (tmin == -1)
119 {
120 PCL_ERROR ("The ray does not intersect with the bounding box \n");
121 return -1;
122 }
123
124 // ray traversal
125 out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin);
126
127 return 0;
128}
129
130//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131template <typename PointT> int
132pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& occluded_voxels)
133{
134 if (!initialized_)
135 {
136 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
137 return -1;
138 }
139
140 // reserve space for the ray vector
141 int reserve_size = div_b_[0] * div_b_[1] * div_b_[2];
142 occluded_voxels.reserve (reserve_size);
143
144 // iterate over the entire voxel grid
145 for (int kk = min_b_.z (); kk <= max_b_.z (); ++kk)
146 for (int jj = min_b_.y (); jj <= max_b_.y (); ++jj)
147 for (int ii = min_b_.x (); ii <= max_b_.x (); ++ii)
148 {
149 Eigen::Vector3i ijk (ii, jj, kk);
150 // process all free voxels
151 int index = this->getCentroidIndexAt (ijk);
152 if (index == -1)
153 {
154 // estimate direction to target voxel
155 Eigen::Vector4f p = getCentroidCoordinate (ijk);
156 Eigen::Vector4f direction = p - sensor_origin_;
157 direction.normalize ();
158
159 // estimate entry point into the voxel grid
160 float tmin = rayBoxIntersection (sensor_origin_, direction);
161
162 // ray traversal
163 int state = rayTraversal (ijk, sensor_origin_, direction, tmin);
164
165 // if voxel is occluded
166 if (state == 1)
167 occluded_voxels.push_back (ijk);
168 }
169 }
170 return 0;
171}
172
173//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174template <typename PointT> float
176 const Eigen::Vector4f& direction)
177{
178 float tmin, tmax, tymin, tymax, tzmin, tzmax;
179
180 if (direction[0] >= 0)
181 {
182 tmin = (b_min_[0] - origin[0]) / direction[0];
183 tmax = (b_max_[0] - origin[0]) / direction[0];
184 }
185 else
186 {
187 tmin = (b_max_[0] - origin[0]) / direction[0];
188 tmax = (b_min_[0] - origin[0]) / direction[0];
189 }
190
191 if (direction[1] >= 0)
192 {
193 tymin = (b_min_[1] - origin[1]) / direction[1];
194 tymax = (b_max_[1] - origin[1]) / direction[1];
195 }
196 else
197 {
198 tymin = (b_max_[1] - origin[1]) / direction[1];
199 tymax = (b_min_[1] - origin[1]) / direction[1];
200 }
201
202 if ((tmin > tymax) || (tymin > tmax))
203 {
204 PCL_ERROR ("no intersection with the bounding box \n");
205 tmin = -1.0f;
206 return tmin;
207 }
208
209 if (tymin > tmin)
210 tmin = tymin;
211 if (tymax < tmax)
212 tmax = tymax;
213
214 if (direction[2] >= 0)
215 {
216 tzmin = (b_min_[2] - origin[2]) / direction[2];
217 tzmax = (b_max_[2] - origin[2]) / direction[2];
218 }
219 else
220 {
221 tzmin = (b_max_[2] - origin[2]) / direction[2];
222 tzmax = (b_min_[2] - origin[2]) / direction[2];
223 }
224
225 if ((tmin > tzmax) || (tzmin > tmax))
226 {
227 PCL_ERROR ("no intersection with the bounding box \n");
228 tmin = -1.0f;
229 return tmin;
230 }
231
232 if (tzmin > tmin)
233 tmin = tzmin;
234 if (tzmax < tmax)
235 tmax = tzmax;
236
237 return tmin;
238}
239
240//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
241template <typename PointT> int
243 const Eigen::Vector4f& origin,
244 const Eigen::Vector4f& direction,
245 const float t_min)
246{
247 // coordinate of the boundary of the voxel grid
248 Eigen::Vector4f start = origin + t_min * direction;
249
250 // i,j,k coordinate of the voxel were the ray enters the voxel grid
251 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
252
253 // steps in which direction we have to travel in the voxel grid
254 int step_x, step_y, step_z;
255
256 // centroid coordinate of the entry voxel
257 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
258
259 if (direction[0] >= 0)
260 {
261 voxel_max[0] += leaf_size_[0] * 0.5f;
262 step_x = 1;
263 }
264 else
265 {
266 voxel_max[0] -= leaf_size_[0] * 0.5f;
267 step_x = -1;
268 }
269 if (direction[1] >= 0)
270 {
271 voxel_max[1] += leaf_size_[1] * 0.5f;
272 step_y = 1;
273 }
274 else
275 {
276 voxel_max[1] -= leaf_size_[1] * 0.5f;
277 step_y = -1;
278 }
279 if (direction[2] >= 0)
280 {
281 voxel_max[2] += leaf_size_[2] * 0.5f;
282 step_z = 1;
283 }
284 else
285 {
286 voxel_max[2] -= leaf_size_[2] * 0.5f;
287 step_z = -1;
288 }
289
290 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
291 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
292 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
293
294 float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
295 float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
296 float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
297
298 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
299 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
300 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
301 {
302 // check if we reached target voxel
303 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
304 return 0;
305
306 // index of the point in the point cloud
307 int index = this->getCentroidIndexAt (ijk);
308 // check if voxel is occupied, if yes return 1 for occluded
309 if (index != -1)
310 return 1;
311
312 // estimate next voxel
313 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
314 {
315 t_max_x += t_delta_x;
316 ijk[0] += step_x;
317 }
318 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
319 {
320 t_max_y += t_delta_y;
321 ijk[1] += step_y;
322 }
323 else
324 {
325 t_max_z += t_delta_z;
326 ijk[2] += step_z;
327 }
328 }
329 return 0;
330}
331
332//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
333template <typename PointT> int
334pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
335 const Eigen::Vector3i& target_voxel,
336 const Eigen::Vector4f& origin,
337 const Eigen::Vector4f& direction,
338 const float t_min)
339{
340 // reserve space for the ray vector
341 int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
342 out_ray.reserve (reserve_size);
343
344 // coordinate of the boundary of the voxel grid
345 Eigen::Vector4f start = origin + t_min * direction;
346
347 // i,j,k coordinate of the voxel were the ray enters the voxel grid
348 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
349 //Eigen::Vector3i ijk = this->getGridCoordinates (start_x, start_y, start_z);
350
351 // steps in which direction we have to travel in the voxel grid
352 int step_x, step_y, step_z;
353
354 // centroid coordinate of the entry voxel
355 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
356
357 if (direction[0] >= 0)
358 {
359 voxel_max[0] += leaf_size_[0] * 0.5f;
360 step_x = 1;
361 }
362 else
363 {
364 voxel_max[0] -= leaf_size_[0] * 0.5f;
365 step_x = -1;
366 }
367 if (direction[1] >= 0)
368 {
369 voxel_max[1] += leaf_size_[1] * 0.5f;
370 step_y = 1;
371 }
372 else
373 {
374 voxel_max[1] -= leaf_size_[1] * 0.5f;
375 step_y = -1;
376 }
377 if (direction[2] >= 0)
378 {
379 voxel_max[2] += leaf_size_[2] * 0.5f;
380 step_z = 1;
381 }
382 else
383 {
384 voxel_max[2] -= leaf_size_[2] * 0.5f;
385 step_z = -1;
386 }
387
388 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
389 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
390 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
391
392 float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
393 float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
394 float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
395
396 // the index of the cloud (-1 if empty)
397 int result = 0;
398
399 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
400 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
401 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
402 {
403 // add voxel to ray
404 out_ray.push_back (ijk);
405
406 // check if we reached target voxel
407 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
408 break;
409
410 // check if voxel is occupied
411 int index = this->getCentroidIndexAt (ijk);
412 if (index != -1)
413 result = 1;
414
415 // estimate next voxel
416 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
417 {
418 t_max_x += t_delta_x;
419 ijk[0] += step_x;
420 }
421 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
422 {
423 t_max_y += t_delta_y;
424 ijk[1] += step_y;
425 }
426 else
427 {
428 t_max_z += t_delta_z;
429 ijk[2] += step_z;
430 }
431 }
432 return result;
433}
434
435//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
436#define PCL_INSTANTIATE_VoxelGridOcclusionEstimation(T) template class PCL_EXPORTS pcl::VoxelGridOcclusionEstimation<T>;
437
438#endif // PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
void initializeVoxelGrid()
Initialize the voxel grid, needs to be called first Builts the voxel grid and computes additional val...
int occlusionEstimationAll(std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > &occluded_voxels)
Computes the voxel coordinates (i, j, k) of all occluded voxels in the voxel grid.
float rayBoxIntersection(const Eigen::Vector4f &origin, const Eigen::Vector4f &direction)
Returns the scaling value (tmin) were the ray intersects with the voxel grid bounding box.
int rayTraversal(const Eigen::Vector3i &target_voxel, const Eigen::Vector4f &origin, const Eigen::Vector4f &direction, const float t_min)
Returns the state of the target voxel (0 = visible, 1 = occupied) unsing a ray traversal algorithm.
int occlusionEstimation(int &out_state, const Eigen::Vector3i &in_target_voxel)
Computes the state (free = 0, occluded = 1) of the voxel after utilizing a ray traversal algorithm to...