Point Cloud Library (PCL) 1.12.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];
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 {
316 ijk[0] += step_x;
317 }
318 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
319 {
321 ijk[1] += step_y;
322 }
323 else
324 {
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 {
419 ijk[0] += step_x;
420 }
421 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
422 {
424 ijk[1] += step_y;
425 }
426 else
427 {
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_
Iterator class for point clouds with or without given indices.
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...