39 #ifndef PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 40 #define PCL_FILTERS_IMPL_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 42 #include <pcl/common/common.h> 43 #include <pcl/filters/voxel_grid_occlusion_estimation.h> 46 template <
typename Po
intT>
void 53 this->filter (filtered_cloud_);
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]);
64 sensor_origin_ = filtered_cloud_.sensor_origin_;
65 sensor_orientation_ = filtered_cloud_.sensor_orientation_;
69 template <
typename Po
intT>
int 71 const Eigen::Vector3i& in_target_voxel)
75 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
80 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
81 Eigen::Vector4f direction = p - sensor_origin_;
82 direction.normalize ();
85 float tmin = rayBoxIntersection (sensor_origin_, direction);
89 PCL_ERROR (
"The ray does not intersect with the bounding box \n");
94 out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin);
100 template <
typename Po
intT>
int 102 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
103 const Eigen::Vector3i& in_target_voxel)
107 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
112 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
113 Eigen::Vector4f direction = p - sensor_origin_;
114 direction.normalize ();
117 float tmin = rayBoxIntersection (sensor_origin_, direction);
121 PCL_ERROR (
"The ray does not intersect with the bounding box \n");
126 out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin);
132 template <
typename Po
intT>
int 137 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
142 int reserve_size = div_b_[0] * div_b_[1] * div_b_[2];
143 occluded_voxels.reserve (reserve_size);
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)
150 Eigen::Vector3i ijk (ii, jj, kk);
152 int index = this->getCentroidIndexAt (ijk);
156 Eigen::Vector4f p = getCentroidCoordinate (ijk);
157 Eigen::Vector4f direction = p - sensor_origin_;
158 direction.normalize ();
161 float tmin = rayBoxIntersection (sensor_origin_, direction);
164 int state = rayTraversal (ijk, sensor_origin_, direction, tmin);
168 occluded_voxels.push_back (ijk);
175 template <
typename Po
intT>
float 177 const Eigen::Vector4f& direction)
179 float tmin, tmax, tymin, tymax, tzmin, tzmax;
181 if (direction[0] >= 0)
183 tmin = (b_min_[0] - origin[0]) / direction[0];
184 tmax = (b_max_[0] - origin[0]) / direction[0];
188 tmin = (b_max_[0] - origin[0]) / direction[0];
189 tmax = (b_min_[0] - origin[0]) / direction[0];
192 if (direction[1] >= 0)
194 tymin = (b_min_[1] - origin[1]) / direction[1];
195 tymax = (b_max_[1] - origin[1]) / direction[1];
199 tymin = (b_max_[1] - origin[1]) / direction[1];
200 tymax = (b_min_[1] - origin[1]) / direction[1];
203 if ((tmin > tymax) || (tymin > tmax))
205 PCL_ERROR (
"no intersection with the bounding box \n");
215 if (direction[2] >= 0)
217 tzmin = (b_min_[2] - origin[2]) / direction[2];
218 tzmax = (b_max_[2] - origin[2]) / direction[2];
222 tzmin = (b_max_[2] - origin[2]) / direction[2];
223 tzmax = (b_min_[2] - origin[2]) / direction[2];
226 if ((tmin > tzmax) || (tzmin > tmax))
228 PCL_ERROR (
"no intersection with the bounding box \n");
242 template <
typename Po
intT>
int 244 const Eigen::Vector4f& origin,
245 const Eigen::Vector4f& direction,
249 Eigen::Vector4f start = origin + t_min * direction;
252 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
255 int step_x, step_y, step_z;
258 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
260 if (direction[0] >= 0)
262 voxel_max[0] += leaf_size_[0] * 0.5f;
267 voxel_max[0] -= leaf_size_[0] * 0.5f;
270 if (direction[1] >= 0)
272 voxel_max[1] += leaf_size_[1] * 0.5f;
277 voxel_max[1] -= leaf_size_[1] * 0.5f;
280 if (direction[2] >= 0)
282 voxel_max[2] += leaf_size_[2] * 0.5f;
287 voxel_max[2] -= leaf_size_[2] * 0.5f;
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];
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]));
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]) )
307 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
311 index = this->getCentroidIndexAt (ijk);
316 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
318 t_max_x += t_delta_x;
321 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
323 t_max_y += t_delta_y;
328 t_max_z += t_delta_z;
336 template <
typename Po
intT>
int 338 const Eigen::Vector3i& target_voxel,
339 const Eigen::Vector4f& origin,
340 const Eigen::Vector4f& direction,
344 int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
345 out_ray.reserve (reserve_size);
348 Eigen::Vector4f start = origin + t_min * direction;
351 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
355 int step_x, step_y, step_z;
358 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
360 if (direction[0] >= 0)
362 voxel_max[0] += leaf_size_[0] * 0.5f;
367 voxel_max[0] -= leaf_size_[0] * 0.5f;
370 if (direction[1] >= 0)
372 voxel_max[1] += leaf_size_[1] * 0.5f;
377 voxel_max[1] -= leaf_size_[1] * 0.5f;
380 if (direction[2] >= 0)
382 voxel_max[2] += leaf_size_[2] * 0.5f;
387 voxel_max[2] -= leaf_size_[2] * 0.5f;
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];
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]));
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]) )
408 out_ray.push_back (ijk);
411 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
415 index = this->getCentroidIndexAt (ijk);
420 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
422 t_max_x += t_delta_x;
425 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
427 t_max_y += t_delta_y;
432 t_max_z += t_delta_z;
440 #define PCL_INSTANTIATE_VoxelGridOcclusionEstimation(T) template class PCL_EXPORTS pcl::VoxelGridOcclusionEstimation<T>; 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...