Point Cloud Library (PCL) 1.15.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 Eigen::Vector3i ijk = this->getGridCoordinates(sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());
67 // first check whether the sensor origin is within the voxel grid bounding box, then whether it is occupied
68 if((ijk[0]>=min_b_[0] && ijk[1]>=min_b_[1] && ijk[2]>=min_b_[2] && ijk[0]<=max_b_[0] && ijk[1]<=max_b_[1] && ijk[2]<=max_b_[2]) && this->getCentroidIndexAt (ijk) != -1) {
69 PCL_WARN ("[VoxelGridOcclusionEstimation::initializeVoxelGrid] The voxel containing the sensor origin (%g, %g, %g) is marked as occupied. We will instead assume that it is free, to be able to perform the occlusion estimation.\n", sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());
70 this->leaf_layout_[((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (this->divb_mul_)] = -1;
71 }
72}
73
74//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75template <typename PointT> int
77 const Eigen::Vector3i& in_target_voxel)
78{
79 if (!initialized_)
80 {
81 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
82 return -1;
83 }
84
85 // estimate direction to target voxel
86 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
87 Eigen::Vector4f direction = p - sensor_origin_;
88 direction.normalize ();
89
90 // estimate entry point into the voxel grid
91 float tmin = rayBoxIntersection (sensor_origin_, direction);
92
93 if (tmin == -1)
94 {
95 PCL_ERROR ("The ray does not intersect with the bounding box \n");
96 return -1;
97 }
98
99 // ray traversal
100 out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin);
101
102 return 0;
103}
104
105//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
106template <typename PointT> int
108 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
109 const Eigen::Vector3i& in_target_voxel)
110{
111 if (!initialized_)
112 {
113 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
114 return -1;
115 }
116
117 // estimate direction to target voxel
118 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
119 Eigen::Vector4f direction = p - sensor_origin_;
120 direction.normalize ();
121
122 // estimate entry point into the voxel grid
123 float tmin = rayBoxIntersection (sensor_origin_, direction);
124
125 if (tmin == -1)
126 {
127 PCL_ERROR ("The ray does not intersect with the bounding box \n");
128 return -1;
129 }
130
131 // ray traversal
132 out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin);
133
134 return 0;
135}
136
137//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
138template <typename PointT> int
139pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& occluded_voxels)
140{
141 if (!initialized_)
142 {
143 PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n");
144 return -1;
145 }
146
147 // reserve space for the ray vector
148 int reserve_size = div_b_[0] * div_b_[1] * div_b_[2];
149 occluded_voxels.reserve (reserve_size);
150
151 // iterate over the entire voxel grid
152 for (int kk = min_b_.z (); kk <= max_b_.z (); ++kk)
153 for (int jj = min_b_.y (); jj <= max_b_.y (); ++jj)
154 for (int ii = min_b_.x (); ii <= max_b_.x (); ++ii)
155 {
156 Eigen::Vector3i ijk (ii, jj, kk);
157 // process all free voxels
158 int index = this->getCentroidIndexAt (ijk);
159 if (index == -1)
160 {
161 // estimate direction to target voxel
162 Eigen::Vector4f p = getCentroidCoordinate (ijk);
163 Eigen::Vector4f direction = p - sensor_origin_;
164 direction.normalize ();
165
166 // estimate entry point into the voxel grid
167 float tmin = rayBoxIntersection (sensor_origin_, direction);
168
169 // ray traversal
170 int state = rayTraversal (ijk, sensor_origin_, direction, tmin);
171
172 // if voxel is occluded
173 if (state == 1)
174 occluded_voxels.push_back (ijk);
175 }
176 }
177 return 0;
178}
179
180//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
181template <typename PointT> float
183 const Eigen::Vector4f& direction)
184{
185 float tmin, tmax, tymin, tymax, tzmin, tzmax;
186
187 if (direction[0] >= 0)
188 {
189 tmin = (b_min_[0] - origin[0]) / direction[0];
190 tmax = (b_max_[0] - origin[0]) / direction[0];
191 }
192 else
193 {
194 tmin = (b_max_[0] - origin[0]) / direction[0];
195 tmax = (b_min_[0] - origin[0]) / direction[0];
196 }
197
198 if (direction[1] >= 0)
199 {
200 tymin = (b_min_[1] - origin[1]) / direction[1];
201 tymax = (b_max_[1] - origin[1]) / direction[1];
202 }
203 else
204 {
205 tymin = (b_max_[1] - origin[1]) / direction[1];
206 tymax = (b_min_[1] - origin[1]) / direction[1];
207 }
208
209 if ((tmin > tymax) || (tymin > tmax))
210 {
211 PCL_ERROR ("no intersection with the bounding box \n");
212 tmin = -1.0f;
213 return tmin;
214 }
215
216 if (tymin > tmin)
217 tmin = tymin;
218 if (tymax < tmax)
219 tmax = tymax;
220
221 if (direction[2] >= 0)
222 {
223 tzmin = (b_min_[2] - origin[2]) / direction[2];
224 tzmax = (b_max_[2] - origin[2]) / direction[2];
225 }
226 else
227 {
228 tzmin = (b_max_[2] - origin[2]) / direction[2];
229 tzmax = (b_min_[2] - origin[2]) / direction[2];
230 }
231
232 if ((tmin > tzmax) || (tzmin > tmax))
233 {
234 PCL_ERROR ("no intersection with the bounding box \n");
235 tmin = -1.0f;
236 return tmin;
237 }
238
239 if (tzmin > tmin)
240 tmin = tzmin;
241 if (tzmax < tmax)
242 tmax = tzmax;
243 return std::max<float>(tmin, 0.0f); // We only want to go in "positive" direction here, not in negative. This is relevant if the origin is inside the bounding box
244}
245
246//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
247template <typename PointT> int
249 const Eigen::Vector4f& origin,
250 const Eigen::Vector4f& direction,
251 const float t_min)
252{
253 // coordinate of the boundary of the voxel grid. To avoid numerical imprecision
254 // causing the start voxel index to be off by one, we add the machine epsilon
255 Eigen::Vector4f start = origin + (t_min > 0.0f ? (t_min + std::numeric_limits<float>::epsilon()) : t_min) * direction;
256
257 // i,j,k coordinate of the voxel were the ray enters the voxel grid
258 Eigen::Vector3i ijk = this->getGridCoordinates(start[0], start[1], start[2]);
259
260 // steps in which direction we have to travel in the voxel grid
261 int step_x, step_y, step_z;
262
263 // centroid coordinate of the entry voxel
264 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
265
266 if (direction[0] >= 0)
267 {
268 voxel_max[0] += leaf_size_[0] * 0.5f;
269 step_x = 1;
270 }
271 else
272 {
273 voxel_max[0] -= leaf_size_[0] * 0.5f;
274 step_x = -1;
275 }
276 if (direction[1] >= 0)
277 {
278 voxel_max[1] += leaf_size_[1] * 0.5f;
279 step_y = 1;
280 }
281 else
282 {
283 voxel_max[1] -= leaf_size_[1] * 0.5f;
284 step_y = -1;
285 }
286 if (direction[2] >= 0)
287 {
288 voxel_max[2] += leaf_size_[2] * 0.5f;
289 step_z = 1;
290 }
291 else
292 {
293 voxel_max[2] -= leaf_size_[2] * 0.5f;
294 step_z = -1;
295 }
296
297 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
298 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
299 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
300
301 float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
302 float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
303 float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
304
305 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
306 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
307 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
308 {
309 // check if we reached target voxel
310 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
311 return 0;
312
313 // index of the point in the point cloud
314 int index = this->getCentroidIndexAt (ijk);
315 // check if voxel is occupied, if yes return 1 for occluded
316 if (index != -1)
317 return 1;
318
319 // estimate next voxel
320 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
321 {
322 t_max_x += t_delta_x;
323 ijk[0] += step_x;
324 }
325 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
326 {
327 t_max_y += t_delta_y;
328 ijk[1] += step_y;
329 }
330 else
331 {
332 t_max_z += t_delta_z;
333 ijk[2] += step_z;
334 }
335 }
336 return 0;
337}
338
339//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
340template <typename PointT> int
341pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
342 const Eigen::Vector3i& target_voxel,
343 const Eigen::Vector4f& origin,
344 const Eigen::Vector4f& direction,
345 const float t_min)
346{
347 // reserve space for the ray vector
348 int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
349 out_ray.reserve (reserve_size);
350
351 // coordinate of the boundary of the voxel grid. To avoid numerical imprecision
352 // causing the start voxel index to be off by one, we add the machine epsilon
353 Eigen::Vector4f start = origin + (t_min > 0.0f ? (t_min + std::numeric_limits<float>::epsilon()) : t_min) * direction;
354
355 // i,j,k coordinate of the voxel were the ray enters the voxel grid
356 Eigen::Vector3i ijk = this->getGridCoordinates (start[0], start[1], start[2]);
357
358 // steps in which direction we have to travel in the voxel grid
359 int step_x, step_y, step_z;
360
361 // centroid coordinate of the entry voxel
362 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
363
364 if (direction[0] >= 0)
365 {
366 voxel_max[0] += leaf_size_[0] * 0.5f;
367 step_x = 1;
368 }
369 else
370 {
371 voxel_max[0] -= leaf_size_[0] * 0.5f;
372 step_x = -1;
373 }
374 if (direction[1] >= 0)
375 {
376 voxel_max[1] += leaf_size_[1] * 0.5f;
377 step_y = 1;
378 }
379 else
380 {
381 voxel_max[1] -= leaf_size_[1] * 0.5f;
382 step_y = -1;
383 }
384 if (direction[2] >= 0)
385 {
386 voxel_max[2] += leaf_size_[2] * 0.5f;
387 step_z = 1;
388 }
389 else
390 {
391 voxel_max[2] -= leaf_size_[2] * 0.5f;
392 step_z = -1;
393 }
394
395 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
396 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
397 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
398
399 float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
400 float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
401 float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
402
403 // the index of the cloud (-1 if empty)
404 int result = 0;
405
406 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
407 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
408 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
409 {
410 // add voxel to ray
411 out_ray.push_back (ijk);
412
413 // check if we reached target voxel
414 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
415 break;
416
417 // check if voxel is occupied
418 int index = this->getCentroidIndexAt (ijk);
419 if (index != -1)
420 result = 1;
421
422 // estimate next voxel
423 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
424 {
425 t_max_x += t_delta_x;
426 ijk[0] += step_x;
427 }
428 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
429 {
430 t_max_y += t_delta_y;
431 ijk[1] += step_y;
432 }
433 else
434 {
435 t_max_z += t_delta_z;
436 ijk[2] += step_z;
437 }
438 }
439 return result;
440}
441
442//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
443#define PCL_INSTANTIATE_VoxelGridOcclusionEstimation(T) template class PCL_EXPORTS pcl::VoxelGridOcclusionEstimation<T>;
444
445#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) using 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...