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_;
66 Eigen::Vector3i ijk = this->getGridCoordinates(sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());
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;
77 const Eigen::Vector3i& in_target_voxel)
81 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
86 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
87 Eigen::Vector4f direction = p - sensor_origin_;
88 direction.normalize ();
91 float tmin = rayBoxIntersection (sensor_origin_, direction);
95 PCL_ERROR (
"The ray does not intersect with the bounding box \n");
100 out_state = rayTraversal (in_target_voxel, sensor_origin_, direction, tmin);
108 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
109 const Eigen::Vector3i& in_target_voxel)
113 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
118 Eigen::Vector4f p = getCentroidCoordinate (in_target_voxel);
119 Eigen::Vector4f direction = p - sensor_origin_;
120 direction.normalize ();
123 float tmin = rayBoxIntersection (sensor_origin_, direction);
127 PCL_ERROR (
"The ray does not intersect with the bounding box \n");
132 out_state = rayTraversal (out_ray, in_target_voxel, sensor_origin_, direction, tmin);
143 PCL_ERROR (
"Voxel grid not initialized; call initializeVoxelGrid () first! \n");
148 int reserve_size = div_b_[0] * div_b_[1] * div_b_[2];
149 occluded_voxels.reserve (reserve_size);
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)
156 Eigen::Vector3i ijk (ii, jj, kk);
158 int index = this->getCentroidIndexAt (ijk);
162 Eigen::Vector4f p = getCentroidCoordinate (ijk);
163 Eigen::Vector4f direction = p - sensor_origin_;
164 direction.normalize ();
167 float tmin = rayBoxIntersection (sensor_origin_, direction);
170 int state = rayTraversal (ijk, sensor_origin_, direction, tmin);
174 occluded_voxels.push_back (ijk);
183 const Eigen::Vector4f& direction)
185 float tmin, tmax, tymin, tymax, tzmin, tzmax;
187 if (direction[0] >= 0)
189 tmin = (b_min_[0] - origin[0]) / direction[0];
190 tmax = (b_max_[0] - origin[0]) / direction[0];
194 tmin = (b_max_[0] - origin[0]) / direction[0];
195 tmax = (b_min_[0] - origin[0]) / direction[0];
198 if (direction[1] >= 0)
200 tymin = (b_min_[1] - origin[1]) / direction[1];
201 tymax = (b_max_[1] - origin[1]) / direction[1];
205 tymin = (b_max_[1] - origin[1]) / direction[1];
206 tymax = (b_min_[1] - origin[1]) / direction[1];
209 if ((tmin > tymax) || (tymin > tmax))
211 PCL_ERROR (
"no intersection with the bounding box \n");
221 if (direction[2] >= 0)
223 tzmin = (b_min_[2] - origin[2]) / direction[2];
224 tzmax = (b_max_[2] - origin[2]) / direction[2];
228 tzmin = (b_max_[2] - origin[2]) / direction[2];
229 tzmax = (b_min_[2] - origin[2]) / direction[2];
232 if ((tmin > tzmax) || (tzmin > tmax))
234 PCL_ERROR (
"no intersection with the bounding box \n");
243 return std::max<float>(tmin, 0.0f);
249 const Eigen::Vector4f& origin,
250 const Eigen::Vector4f& direction,
254 Eigen::Vector4f start = origin + t_min * direction;
257 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
260 int step_x, step_y, step_z;
263 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
265 if (direction[0] >= 0)
267 voxel_max[0] += leaf_size_[0] * 0.5f;
272 voxel_max[0] -= leaf_size_[0] * 0.5f;
275 if (direction[1] >= 0)
277 voxel_max[1] += leaf_size_[1] * 0.5f;
282 voxel_max[1] -= leaf_size_[1] * 0.5f;
285 if (direction[2] >= 0)
287 voxel_max[2] += leaf_size_[2] * 0.5f;
292 voxel_max[2] -= leaf_size_[2] * 0.5f;
296 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
297 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
298 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
300 float t_delta_x = leaf_size_[0] /
static_cast<float> (std::abs (direction[0]));
301 float t_delta_y = leaf_size_[1] /
static_cast<float> (std::abs (direction[1]));
302 float t_delta_z = leaf_size_[2] /
static_cast<float> (std::abs (direction[2]));
304 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
305 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
306 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
309 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
313 int index = this->getCentroidIndexAt (ijk);
319 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
321 t_max_x += t_delta_x;
324 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
326 t_max_y += t_delta_y;
331 t_max_z += t_delta_z;
341 const Eigen::Vector3i& target_voxel,
342 const Eigen::Vector4f& origin,
343 const Eigen::Vector4f& direction,
347 int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
348 out_ray.reserve (reserve_size);
351 Eigen::Vector4f start = origin + t_min * direction;
354 Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
358 int step_x, step_y, step_z;
361 Eigen::Vector4f voxel_max = getCentroidCoordinate (ijk);
363 if (direction[0] >= 0)
365 voxel_max[0] += leaf_size_[0] * 0.5f;
370 voxel_max[0] -= leaf_size_[0] * 0.5f;
373 if (direction[1] >= 0)
375 voxel_max[1] += leaf_size_[1] * 0.5f;
380 voxel_max[1] -= leaf_size_[1] * 0.5f;
383 if (direction[2] >= 0)
385 voxel_max[2] += leaf_size_[2] * 0.5f;
390 voxel_max[2] -= leaf_size_[2] * 0.5f;
394 float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
395 float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
396 float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
398 float t_delta_x = leaf_size_[0] /
static_cast<float> (std::abs (direction[0]));
399 float t_delta_y = leaf_size_[1] /
static_cast<float> (std::abs (direction[1]));
400 float t_delta_z = leaf_size_[2] /
static_cast<float> (std::abs (direction[2]));
405 while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
406 (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
407 (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
410 out_ray.push_back (ijk);
413 if (ijk[0] == target_voxel[0] && ijk[1] == target_voxel[1] && ijk[2] == target_voxel[2])
417 int index = this->getCentroidIndexAt (ijk);
422 if(t_max_x <= t_max_y && t_max_x <= t_max_z)
424 t_max_x += t_delta_x;
427 else if(t_max_y <= t_max_z && t_max_y <= t_max_x)
429 t_max_y += t_delta_y;
434 t_max_z += t_delta_z;
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.