fix octree bounds (#1532)

- fixed octree bounds reducing, for some resolution/bounds ratio octree bounds can be unexpectedly reduced
- added unit test for octree bounds correction
- fix tree bounds enlarge due to precision issues + unit test, for some testcases it can change tree depth and break tree deserialization

octree/include/pcl/octree/impl/octree_pointcloud.hpp

@@ -620,9 +620,9 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   const float minValue = std::numeric_limits<float>::epsilon();
 
   // find maximum key values for x, y, z
-  max_key_x = static_cast<unsigned int> ((max_x_ - min_x_) / resolution_);
-  max_key_y = static_cast<unsigned int> ((max_y_ - min_y_) / resolution_);
-  max_key_z = static_cast<unsigned int> ((max_z_ - min_z_) / resolution_);
+  max_key_x = static_cast<unsigned int> (ceil ((max_x_ - min_x_ - minValue) / resolution_));
+  max_key_y = static_cast<unsigned int> (ceil ((max_y_ - min_y_ - minValue) / resolution_));
+  max_key_z = static_cast<unsigned int> (ceil ((max_z_ - min_z_ - minValue) / resolution_));
 
   // find maximum amount of keys
   max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
@@ -632,7 +632,7 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))),
                                   static_cast<unsigned int> (0));
 
-  octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_-minValue;
+  octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_;
 
   if (this->leaf_count_ == 0)
   {
@@ -644,13 +644,25 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
     octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
 
-    min_x_ -= octree_oversize_x;
-    min_y_ -= octree_oversize_y;
-    min_z_ -= octree_oversize_z;
+    assert (octree_oversize_x > -minValue);
+    assert (octree_oversize_y > -minValue);
+    assert (octree_oversize_z > -minValue);
 
-    max_x_ += octree_oversize_x;
-    max_y_ += octree_oversize_y;
-    max_z_ += octree_oversize_z;
+    if (octree_oversize_x > minValue)
+    {
+      min_x_ -= octree_oversize_x;
+      max_x_ += octree_oversize_x;
+    }
+    if (octree_oversize_y > minValue)
+    {
+      min_y_ -= octree_oversize_y;
+      max_y_ += octree_oversize_y;
+    }
+    if (octree_oversize_z > minValue)
+    {
+      min_z_ -= octree_oversize_z;
+      max_z_ += octree_oversize_z;
+    }
   }
   else
   {
@@ -673,6 +685,10 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     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);
   }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

test/features/test_pfh_estimation.cpp

@@ -569,7 +569,7 @@ TEST (PCL, GFPFH)
   PointCloud<GFPFHSignature16> descriptor;
   gfpfh.compute (descriptor);
 
-  const float ref_values[] = { 3216, 7760, 8740, 26584, 4645, 2995, 3029, 4349, 6192, 5440, 9514, 47563, 21814, 22073, 5734, 1253 };
+  const float ref_values[] = { 1877, 6375, 5361, 14393, 6674, 2471, 2248, 2753, 3117, 4585, 14388, 32407, 15122, 3061, 3202, 794 };
 
   EXPECT_EQ (descriptor.points.size (), 1);
   for (size_t i = 0; i < size_t (descriptor.points[0].descriptorSize ()); ++i)

test/octree/test_octree.cpp

@@ -1699,6 +1699,38 @@ TEST (PCL, Octree_Pointcloud_Adjacency)
   }
 
 }
+
+TEST (PCL, Octree_Pointcloud_Bounds)
+{
+    const double SOME_RESOLUTION (10 + 1/3.0);
+    const int SOME_DEPTH (4);
+    const double DESIRED_MAX = ((1<<SOME_DEPTH) + 0.5)*SOME_RESOLUTION;
+    const double DESIRED_MIN = 0;
+
+    OctreePointCloud<PointXYZ> tree (SOME_RESOLUTION);
+    tree.defineBoundingBox (DESIRED_MIN, DESIRED_MIN, DESIRED_MIN, DESIRED_MAX, DESIRED_MAX, DESIRED_MAX);
+
+    double min_x, min_y, min_z, max_x, max_y, max_z;
+    tree.getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
+
+    ASSERT_GE( max_x, DESIRED_MAX );
+    ASSERT_GE( DESIRED_MIN, min_x );
+
+    const double LARGE_MIN = 1e7-45*SOME_RESOLUTION;
+    const double LARGE_MAX = 1e7-5*SOME_RESOLUTION;
+    tree.defineBoundingBox (LARGE_MIN, LARGE_MIN, LARGE_MIN, LARGE_MAX, LARGE_MAX, LARGE_MAX);
+    tree.getBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
+    const unsigned int depth = tree.getTreeDepth ();
+    tree.defineBoundingBox (min_x, min_y, min_z, max_x, max_y, max_z);
+
+    ASSERT_EQ( depth == tree.getTreeDepth (), true );
+
+    double min_x2, min_y2, min_z2, max_x2, max_y2, max_z2;
+    tree.getBoundingBox (min_x2, min_y2, min_z2, max_x2, max_y2, max_z2);
+
+    ASSERT_EQ( min_x2 == min_x, true );
+    ASSERT_EQ( max_x2 == max_x, true );
+}
 /* ---[ */
 int
 main (int argc, char** argv)