Point Cloud Library (PCL)  1.14.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  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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
106 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
138 template <typename PointT> int
139 pcl::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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
181 template <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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
247 template <typename PointT> int
248 pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i& target_voxel,
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 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
340 template <typename PointT> int
341 pcl::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...