Skip to content

Commit

Permalink
VoxelGrid coordinate to index mappoing should always round down
Browse files Browse the repository at this point in the history
  • Loading branch information
themightyoarfish committed Jan 26, 2024
1 parent 910ccc6 commit b29ebf4
Showing 1 changed file with 15 additions and 15 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::initializeVoxelGrid ()
{
// initialization set to true
initialized_ = true;

// create the voxel grid and store the output cloud
this->filter (filtered_cloud_);

Expand Down Expand Up @@ -162,13 +162,13 @@ pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<E
Eigen::Vector4f p = getCentroidCoordinate (ijk);
Eigen::Vector4f direction = p - sensor_origin_;
direction.normalize ();

// estimate entry point into the voxel grid
float tmin = rayBoxIntersection (sensor_origin_, direction);

// ray traversal
int state = rayTraversal (ijk, sensor_origin_, direction, tmin);

// if voxel is occluded
if (state == 1)
occluded_voxels.push_back (ijk);
Expand All @@ -179,7 +179,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<E

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> float
pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vector4f& origin,
pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction)
{
float tmin, tmax, tymin, tymax, tzmin, tzmax;
Expand All @@ -198,7 +198,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vect
if (direction[1] >= 0)
{
tymin = (b_min_[1] - origin[1]) / direction[1];
tymax = (b_max_[1] - origin[1]) / direction[1];
tymax = (b_max_[1] - origin[1]) / direction[1];
}
else
{
Expand Down Expand Up @@ -233,7 +233,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vect
{
PCL_ERROR ("no intersection with the bounding box \n");
tmin = -1.0f;
return tmin;
return tmin;
}

if (tzmin > tmin)
Expand All @@ -246,7 +246,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vect
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i& target_voxel,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction,
const float t_min)
{
Expand Down Expand Up @@ -296,13 +296,13 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i&
float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];

float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));

while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
(ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
(ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
(ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
{
// check if we reached target voxel
Expand Down Expand Up @@ -339,7 +339,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i&
template <typename PointT> int
pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
const Eigen::Vector3i& target_voxel,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& origin,
const Eigen::Vector4f& direction,
const float t_min)
{
Expand All @@ -351,7 +351,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vect
Eigen::Vector4f start = origin + t_min * direction;

// i,j,k coordinate of the voxel were the ray enters the voxel grid
Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
Eigen::Vector3i ijk = this->getGridCoordinates (start[0], start[1], start[2]);
//Eigen::Vector3i ijk = this->getGridCoordinates (start_x, start_y, start_z);

// steps in which direction we have to travel in the voxel grid
Expand Down Expand Up @@ -394,16 +394,16 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vect
float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];

float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));

// the index of the cloud (-1 if empty)
int result = 0;

while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
(ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
(ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
(ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
{
// add voxel to ray
Expand Down

0 comments on commit b29ebf4

Please sign in to comment.