Point Cloud Library (PCL)  1.12.1-dev
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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
99 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131 template <typename PointT> int
132 pcl::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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
241 template <typename PointT> int
242 pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i& target_voxel,
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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
333 template <typename PointT> int
334 pcl::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...