39 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
40 #define PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_
42 #include <pcl/filters/voxel_grid_occlusion_estimation.h>
45 template <
typename Po
intT>
void
52 this->filter (filtered_cloud_);
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]);
63 sensor_origin_ = filtered_cloud_.sensor_origin_;
64 sensor_orientation_ = filtered_cloud_.sensor_orientation_;
68 template <
typename Po
intT>
int
70 const Eigen::Vector3i& in_target_voxel)
74 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
79 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
80 Eigen::Vector4f direction = p - sensor_origin_;
81 direction.normalize ();
84 float tmin = rayBoxIntersection (sensor_origin_, direction);
88 PCL_ERROR (
"The ray does not intersect with the bounding box \n");
93 out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin);
99 template <
typename Po
intT>
int
101 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
102 const Eigen::Vector3i& in_target_voxel)
106 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
111 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
112 Eigen::Vector4f direction = p - sensor_origin_;
113 direction.normalize ();
116 float tmin = rayBoxIntersection (sensor_origin_, direction);
120 PCL_ERROR (
"The ray does not intersect with the bounding box \n");
125 out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin);
131 template <
typename Po
intT>
int
136 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
141 int reserve_size = div_b_[0] * div_b_[1] * div_b_[2];
142 occluded_voxels.reserve (reserve_size);
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)
149 Eigen::Vector3i ijk (ii, jj, kk);
151 int index = this->getCentroidIndexAt (ijk);
155 Eigen::Vector4f p = getCentroidCoordinate (ijk);
156 Eigen::Vector4f direction = p - sensor_origin_;
157 direction.normalize ();
160 float tmin = rayBoxIntersection (sensor_origin_, direction);
163 int state = rayTraversal (ijk, sensor_origin_, direction, tmin);
167 occluded_voxels.push_back (ijk);
174 template <
typename Po
intT>
float
176 const Eigen::Vector4f& direction)
178 float tmin, tmax, tymin, tymax, tzmin, tzmax;
180 if (direction[0] >= 0)
182 tmin = (b_min_[0] - origin[0]) / direction[0];
183 tmax = (b_max_[0] - origin[0]) / direction[0];
187 tmin = (b_max_[0] - origin[0]) / direction[0];
188 tmax = (b_min_[0] - origin[0]) / direction[0];
191 if (direction[1] >= 0)
193 tymin = (b_min_[1] - origin[1]) / direction[1];
194 tymax = (b_max_[1] - origin[1]) / direction[1];
198 tymin = (b_max_[1] - origin[1]) / direction[1];
199 tymax = (b_min_[1] - origin[1]) / direction[1];
202 if ((tmin > tymax) || (tymin > tmax))
204 PCL_ERROR (
"no intersection with the bounding box \n");
214 if (direction[2] >= 0)
216 tzmin = (b_min_[2] - origin[2]) / direction[2];
217 tzmax = (b_max_[2] - origin[2]) / direction[2];
221 tzmin = (b_max_[2] - origin[2]) / direction[2];
222 tzmax = (b_min_[2] - origin[2]) / direction[2];
225 if ((tmin > tzmax) || (tzmin > tmax))
227 PCL_ERROR (
"no intersection with the bounding box \n");
241 template <
typename Po
intT>
int
243 const Eigen::Vector4f& origin,
244 const Eigen::Vector4f& direction,
248 Eigen::Vector4f start = origin + t_min * direction;
251 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
254 int step_x, step_y, step_z;
257 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
259 if (direction[0] >= 0)
261 voxel_max[0] += leaf_size_[0] * 0.5f;
266 voxel_max[0] -= leaf_size_[0] * 0.5f;
269 if (direction[1] >= 0)
271 voxel_max[1] += leaf_size_[1] * 0.5f;
276 voxel_max[1] -= leaf_size_[1] * 0.5f;
279 if (direction[2] >= 0)
281 voxel_max[2] += leaf_size_[2] * 0.5f;
286 voxel_max[2] -= leaf_size_[2] * 0.5f;
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];
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]));
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]) )
303 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
307 int index = this->getCentroidIndexAt (ijk);
313 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
315 t_max_x += t_delta_x;
318 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
320 t_max_y += t_delta_y;
325 t_max_z += t_delta_z;
333 template <
typename Po
intT>
int
335 const Eigen::Vector3i& target_voxel,
336 const Eigen::Vector4f& origin,
337 const Eigen::Vector4f& direction,
341 int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
342 out_ray.reserve (reserve_size);
345 Eigen::Vector4f start = origin + t_min * direction;
348 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
352 int step_x, step_y, step_z;
355 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
357 if (direction[0] >= 0)
359 voxel_max[0] += leaf_size_[0] * 0.5f;
364 voxel_max[0] -= leaf_size_[0] * 0.5f;
367 if (direction[1] >= 0)
369 voxel_max[1] += leaf_size_[1] * 0.5f;
374 voxel_max[1] -= leaf_size_[1] * 0.5f;
377 if (direction[2] >= 0)
379 voxel_max[2] += leaf_size_[2] * 0.5f;
384 voxel_max[2] -= leaf_size_[2] * 0.5f;
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];
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]));
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]) )
404 out_ray.push_back (ijk);
407 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
411 int index = this->getCentroidIndexAt (ijk);
416 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
418 t_max_x += t_delta_x;
421 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
423 t_max_y += t_delta_y;
428 t_max_z += t_delta_z;
436 #define PCL_INSTANTIATE_VoxelGridOcclusionEstimation(T) template class PCL_EXPORTS pcl::VoxelGridOcclusionEstimation<T>;
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...