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_;
101 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >&
out_ray,
106 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
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");
136 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
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)
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);
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]);
257 Eigen::Vector4f
voxel_max = getCentroidCoordinate (
ijk);
259 if (direction[0] >= 0)
269 if (direction[1] >= 0)
279 if (direction[2] >= 0)
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]) )
307 int index = this->getCentroidIndexAt (
ijk);
336 const Eigen::Vector4f&
origin,
337 const Eigen::Vector4f& direction,
341 int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
345 Eigen::Vector4f start =
origin +
t_min * direction;
348 Eigen::Vector3i
ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
355 Eigen::Vector4f
voxel_max = getCentroidCoordinate (
ijk);
357 if (direction[0] >= 0)
367 if (direction[1] >= 0)
377 if (direction[2] >= 0)
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]) )
411 int index = this->getCentroidIndexAt (
ijk);
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.