[filters] VoxelGridOcclusionEstimation not working for some inputs

[themightyoarfish]

I have a sample point cloud in which occlusion estimation says nothing is occluded, regardless of voxel size, which is untrue. I am confused by this because it works on other point clouds.

Context

After the recent fixes to VoxelGridOcclusionEstimation, I have used this filter and it seemed to be working as expected, provided I set the voxel size big enough to actually occlude something. But now I ran across a sample where its obvious that some space is occluded, but everything is marked free.

A screenshot is here, the upper red dot marks the sensor origin in the cloud, the lower one is just 1 voxel I am casting to

Expected behavior

Many voxels occluded

Current Behavior

Nothing occluded

To Reproduce

Compile 910ccc61737612daad06e11fc79e51540d6480fc (current master), then run

./build/bin/pcl_voxel_grid_occlusion_estimation wat.pcd -leaf 0.2,0.2,0.2

or any other voxel size. The result is always like

> Ray-Traversal [done, 0.005125 ms : 0 occluded voxels]

Your Environment (please complete the following information):

• OS: macos 12.6.5
• Compiler: apple clang-1400.0.29.202
• PCL Version master HEAD

Possible Solution

no idea

wat.pcd.zip

[themightyoarfish]

I'm investigating the code right now, and I can see that inside rayTraversal(), this condition is never fulfilled, so raycast does not start.

  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]) )

First mostly unfounded guess: The problem appears when the sensor origin is inside the maximum X voxel, because it fails on the X coordinate condition in the first iteration, while Y and Z would permit at least one iteration of the loop.

If I add another point beyond the sensor origin to the cloud in X coordinate, the occlusion estimation appears to work.

This is a sample program where i check the voxel occlusion on a test voxel which should be occluded

#include <pcl/impl/point_types.hpp>
#define PCL_NO_PRECOMPILE
#include <pcl/filters/voxel_grid_occlusion_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <vtkObject.h>

using namespace pcl;
using namespace pcl::visualization;
using CloudT = PointCloud<PointXYZ>;
using namespace Eigen;

int main(int argc, char *argv[]) {
  vtkObject::GlobalWarningDisplayOff();

  CloudT::Ptr cloud{new CloudT};
  io::loadPCDFile("/Users/rasmus/Downloads/wat.pcd", *cloud);
  cloud->sensor_origin_ = Vector4f(3.18874, 0.825969, 7.41143, 1);
  // cloud->push_back(PointXYZ(3.2, 0.825969, 7.41143)); // uncomment to make work

  VoxelGridOcclusionEstimation<PointXYZ> vox_occ;
  const float voxel_resolution_ = 0.2;
  vox_occ.setLeafSize(voxel_resolution_, voxel_resolution_, voxel_resolution_);

  vox_occ.setInputCloud(cloud);
  vox_occ.initializeVoxelGrid();

  const Vector3f query_point(0, 0.249516, 0.556921);
  const Vector3i query_voxel = vox_occ.getGridCoordinates(
      query_point.x(), query_point.y(), query_point.z());
  PointXYZ query_point_pcl;
  query_point_pcl.getVector3fMap() = query_point;
  PointXYZ sensor_origin_pcl;
  sensor_origin_pcl.getArray4fMap() = cloud->sensor_origin_;

  int out_state;
  const int success = vox_occ.occlusionEstimation(out_state, query_voxel);

  if (out_state == 1) {
    std::cout << "Voxel is occluded" << std::endl;
  } else {
    std::cout << "Voxel is not occluded" << std::endl;
  }

  PCLVisualizer viewer;

  cloud->sensor_origin_ = Vector4f(0, 0, 0, 1);
  viewer.addPointCloud(cloud);
  viewer.addCoordinateSystem(1);
  viewer.addSphere(sensor_origin_pcl, 0.1, 1, 0, 0, "origin");
  viewer.addSphere(query_point_pcl, 0.1, 1, 0, 0, "query");
  viewer.spin();

  return 0;
}

[mvieth]

It seems to me that this problem appears if the sensor origin is at the border of the bounding box. At first, it is counted as being inside the box, but then in getGridCoordinatesRound, it is assigned to a voxel outside the box, and the while-loop (as you said) never starts. Maybe the round in getGridCoordinatesRound is wrong

[themightyoarfish]

This must be because only X gets rounded up via round(). Y and Z get rounded down.

[themightyoarfish]

What is this code doing anyway? The docstring says

We use round here instead of std::floor due to some numerical issues.

But then the round() function uses std::floor internally. I guess floor(x+0.5) rounds to the nearest integer, so this will round down or up depending. instead don't we want to always round down or up? This concrete case "works" when just using plain floor, for instance, but I have not yet understood what this does.

      /** \brief Returns a rounded value.
        * \param[in] d
        * \return rounded value
        */
      inline float
      round (float d)
      {
        return static_cast<float> (std::floor (d + 0.5));
      }

      // We use round here instead of std::floor due to some numerical issues.
      /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
        * \param[in] x the X point coordinate to get the (i, j, k) index at
        * \param[in] y the Y point coordinate to get the (i, j, k) index at
        * \param[in] z the Z point coordinate to get the (i, j, k) index at
        */
      inline Eigen::Vector3i
      getGridCoordinatesRound (float x, float y, float z)
      {
        return {static_cast<int> (round (x * inverse_leaf_size_[0])),
                                static_cast<int> (round (y * inverse_leaf_size_[1])),
                                static_cast<int> (round (z * inverse_leaf_size_[2]))};
      }

Also the full occlusion estimation of the cloud exhibits expected behaviour when just using std::floor():

[themightyoarfish]

It seems that getGridCoordinatesRound() differs from getGridCoordinates() in that it will sometimes round up to the next voxel, but why does this ever make sense? It's wrong to round up to the next voxel, unless the coordinate is right on the edge, in which case it's a matter of definition. Inside the rayTraversal() function, we find some commented code, so there must've been a reason for this change

  Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
  //Eigen::Vector3i ijk = this->getGridCoordinates (start_x, start_y, start_z);

[themightyoarfish]

From searching the history I can see that all this code has been here since this filter was initially committed, so there won't likely be any explanation.

[mvieth]

I agree that floor makes more sense, but there seems to be some more weird logic (bugs?) in the code. Apparently, this paper was the basis for the code: http://www.cse.yorku.ca/~amana/research/grid.pdf maybe there are some explanations in there. And I started writing a test case (so far, there is none):

TEST (VoxelGridOcclusionEstimation, Filters)
{
  auto input_cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
  input_cloud->emplace_back(0.0, 0.0, 0.0);
  input_cloud->emplace_back(9.9, 9.9, 9.9); // we want a nice bounding box from (0, 0, 0) to (10, 10, 10)
  input_cloud->sensor_origin_ << -0.1, 0.5, 0.5;
  pcl::io::savePCDFileBinary("/tmp/test.pcd", *input_cloud);
  pcl::VoxelGridOcclusionEstimation<pcl::PointXYZ> vg;
  vg.setInputCloud (input_cloud);
  vg.setLeafSize (1.0, 1.0, 1.0);
  vg.initializeVoxelGrid ();
  std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > occluded_voxels;
  vg.occlusionEstimationAll (occluded_voxels);
  std::cout << occluded_voxels.size() << std::endl;
}