[OCTREE] Add bounding box checks in isVoxelOccupiedAtPoint() and deleteVoxelAtPoint()

[frozar]

The function which failed on my computer was Octree_Pointcloud_Test in test/octree/test_octree.cpp. In this function at line 749, we have:

if (!octreeA.isVoxelOccupiedAtPoint (newPoint))

and in this context the octreeA is empty. The first step of the function isVoxelOccupiedAtPoint is to query the key corresponding to the point argument newPoint. The generated key for this point is of course out of the score of the current octree because octreeA is empty.

To fix this test, on one hand the easy way is to remove the assert() call in pcl::octree::OctreePointCloud::genOctreeKeyforPoint() and on the other hand the harder way is to allow pcl::octree::OctreePointCloud::genOctreeKeyforPoint() to return a status (a bool) to be able to check if everything goes well or not: this means to change its prototype. I chose the first solution to minimize the number of changes.

This pull request is relative to issue #677.

[taketwo]

I think we should not remove the assertion, it may be helpful for catching other bugs. My proposal would be to add a tree emptiness check in the beginning of the isVoxelOccupiedAtPoint() function. Indeed, if the tree is empty, this function should return false without even messing with keys and searches. I'm not super familiar with the octree module, but from what I see the emptiness test can be performed by checking leaf_count_ == 0. So I would propose to add this in the beginning of every isVoxelOccupiedAtPoint() overload. Also, looking at the code, deleteVoxelAtPoint() would benefit from such a check as well.

(A small clarification for others reading this issue: this is not a "proper" unit test failure, it's an assertion. This only shows up if compiled in "debug" mode. This is why on Travis everything is fine.)

[frozar]

Short answer

I disagree. Removing the assertions is still the better solution in my opinion.

The real long answer

In the case you keep the assertions and you add a check like this:

pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint (const PointT& point_arg) const
{
  if(this->leaf_count_ == 0) {
    return false;
  }
[...]
}

at runtime in this unit test, you will be able to add the first point but for the second point, things go wrong. The loop inside the test Octree_Pointcloud_Test is:

    for (point = 0; point < pointcount; point++)
    {
      // gereate a random point
      PointXYZ newPoint (static_cast<float> (1024.0 * rand () / RAND_MAX),
                         static_cast<float> (1024.0 * rand () / RAND_MAX),
                         static_cast<float> (1024.0 * rand () / RAND_MAX));

      if (!octreeA.isVoxelOccupiedAtPoint (newPoint))
      {
        // add point only to OctreePointCloudSinglePoint if voxel at point location doesn't exist
        octreeA.addPointToCloud (newPoint, cloudA);
      }

      // OctreePointCloudPointVector can store all points..
      cloudB->push_back (newPoint);
    }

When you check the condition octreeA.isVoxelOccupiedAtPoint (newPoint) for the second point, you enter in the function pcl::octree::OctreePointCloud::genOctreeKeyforPoint(). To be understandable, let copy the code here :

pcl::octree::OctreePointCloud::genOctreeKeyforPoint (const PointT& point_arg,
                                                     OctreeKey & key_arg) const
  {
    // calculate integer key for point coordinates
    key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
    key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
    key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
    
    assert (key_arg.x <= this->max_key_.x);
    assert (key_arg.y <= this->max_key_.y);
    assert (key_arg.z <= this->max_key_.z);
  }

and let see an example of execution.

As input:
  point_arg = {x = 533.892151, y = 541.182373, z = 876.574219}
  this->min_ = {x_ = 796.55121826194227, y_ = 724.49799560569227, z_ = 155.08510803245008}
  this->resolution = 0.01
  octree.max_key_ = {x = 1, y = 1, z = 1}

As output:
  key_arg = {x = 4294941031, y = 4294948965, z = 72148}

Obvously, an assertion comparing octree.max_key_ and key_arg will throw an exception. In this unit test, random points are added on fly in the OctreePointCloud structure (so for example nobody knows the min_x or max_x of the final point cloud points). When the first point was added to the OctreePointCloud structure, only one voxel was created under the root node, so the depth of the octree is of 1 and the octree.max_key_ is tiny. Also the boundaries of the octree structure (fields min_x_, min_y_, min_z_, max_x_, max_y_, max_z_) are updated on fly depending of the input points. The key generated for the second point is huge, as expected I would say.

I don't think that the assert() are legitimate (I would say more, I think they're wrong). In the case where you remove the if(this->leaf_count_ == 0) and the assert(), when you try to add the second point to the OctreePointCloud structure, a huge key_arg is still generated. But if you look at the function addPointIdx() called just after in the loop, you'll see:

pcl::octree::OctreePointCloud::addPointIdx (const int point_idx_arg)
{
[...]
  // make sure bounding box is big enough
  adoptBoundingBoxToPoint (point);

The function `adoptBoundingBoxToPoint(), which takes as input a point, manages to reshape the octree to be able to generate the right voxel for this new point.

Remove assert test are usually a bad idea, but in this particular case, I'm not sure because the right behavior of the class seems to happen without them.

PS: sorry for this too long answer.

[taketwo]

Thanks for the detailed explanation, now I get your point.

Summarizing, the problem is that genOctreeKeyForPoint() effectively asserts that the point for which the key is generated is within the current bounding box. However, from isVoxelOccupiedAtPoint() it can be called with an arbitrary query point that may lay outside the box.

Your solution is to remove the assertion. However, it's still not obvious for me that this check is not needed. Perhaps it was the intention of the author to make sure that key generation happens only for points in the bounding box. (Because otherwise an invalid key is generated and some other function may explode if given such a key.)

I do agree that my previous suggestion with leaf count test is not valid. But here is an alternative. In one of the overloads of the isVoxelOccupiedAtPoint() there is already a check isPointWithinBoundingBox(). However, it happens only after key generation. We can pull it forward so it becomes the first check in the function.

What do you think?

[frozar]

Your suggestion seems to be the good one. Put isPointWithinBoundingBox() before to generate a key is a good idea. My last commit correspond to this solution. If the tests pass, I'll squash my branch with this final solution ;)

[taketwo]

Thanks. Can you also add a check to the deleteVoxelAtPoint function which follows immediately after?

[taketwo]

When the test passes and you squash, please also fix the style.

[taketwo]

{ should go to the new line. Or delete braces completely, no need here.

[taketwo]

Same comment

[frozar]

Things should be fine now.