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