Faster octree radiusSearch

... by improving the check whether an octree node might contain a point within radius. Previously this was done by a sphere around the node center, now this done exact (like a rounded cube).

Benchmarks:
NormalEstimation (without OpenMP) with octree:
| NormalEstimation  | Old       | New        |
|-------------------|-----------|------------|
| r=0.01, cloud=milk| 914 ms    | 756 ms     |
| r=0.01, cloud=mug | 1133 ms   | 881 ms     |
| r=0.02, cloud=milk| 1979 ms   | 1784 ms    |
| r=0.02, cloud=mug | 2642 ms   | 2244 ms    |

Calling radiusSearch on every (valid) point of a cloud (r is tuned so that 5 and 50 neighbours are found on average, respectively):
| Search          | Old       | New        |
|-----------------|-----------|------------|
| r=0.0016, cloud1| 267 ms    | 175 ms     |
| r=0.0022, cloud2| 319 ms    | 210 ms     |
| r=0.0072, cloud3| 421 ms    | 293 ms     |
| r=0.005, cloud1 | 440 ms    | 356 ms     |
| r=0.0068, cloud2| 513 ms    | 441 ms     |
| r=0.024, cloud3 | 877 ms    | 873 ms     |

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

@@ -886,12 +886,9 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   min_pt(2) = static_cast<float>(static_cast<double>(key_arg.z) * voxel_side_len +
                                  this->min_z_);
 
-  max_pt(0) = static_cast<float>(static_cast<double>(key_arg.x + 1) * voxel_side_len +
-                                 this->min_x_);
-  max_pt(1) = static_cast<float>(static_cast<double>(key_arg.y + 1) * voxel_side_len +
-                                 this->min_y_);
-  max_pt(2) = static_cast<float>(static_cast<double>(key_arg.z + 1) * voxel_side_len +
-                                 this->min_z_);
+  max_pt(0) = min_pt(0) + voxel_side_len;
+  max_pt(1) = min_pt(1) + voxel_side_len;
+  max_pt(2) = min_pt(2) + voxel_side_len;
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////

octree/include/pcl/octree/impl/octree_search.hpp

@@ -348,9 +348,6 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
                                       std::vector<float>& k_sqr_distances,
                                       uindex_t max_nn) const
 {
-  // get spatial voxel information
-  double voxel_squared_diameter = this->getVoxelSquaredDiameter(tree_depth);
-
   // iterate over all children
   for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
     if (!this->branchHasChild(*node, child_idx))
@@ -360,25 +357,30 @@ OctreePointCloudSearch<PointT, LeafContainerT, BranchContainerT>::
     child_node = this->getBranchChildPtr(*node, child_idx);
 
     OctreeKey new_key;
-    PointT voxel_center;
     float squared_dist;
 
     // generate new key for current branch voxel
     new_key.x = (key.x << 1) + (!!(child_idx & (1 << 2)));
     new_key.y = (key.y << 1) + (!!(child_idx & (1 << 1)));
     new_key.z = (key.z << 1) + (!!(child_idx & (1 << 0)));
 
-    // generate voxel center point for voxel at key
-    this->genVoxelCenterFromOctreeKey(new_key, tree_depth, voxel_center);
-
-    // calculate distance to search point
-    squared_dist = pointSquaredDist(static_cast<const PointT&>(voxel_center), point);
-
-    // if distance is smaller than search radius
-    if (squared_dist + this->epsilon_ <=
-        voxel_squared_diameter / 4.0 + radiusSquared +
-            sqrt(voxel_squared_diameter * radiusSquared)) {
-
+    // compute min distance between query point and any point in this child node, to decide whether we can skip it
+    Eigen::Vector3f min_pt, max_pt;
+    this->genVoxelBoundsFromOctreeKey(new_key, tree_depth, min_pt, max_pt);
+    squared_dist = 0.0f;
+    if(point.x < min_pt.x())
+      squared_dist += std::pow(point.x-min_pt.x(), 2);
+    else if(point.x > max_pt.x())
+      squared_dist += std::pow(point.x-max_pt.x(), 2);
+    if(point.y < min_pt.y())
+      squared_dist += std::pow(point.y-min_pt.y(), 2);
+    else if(point.y > max_pt.y())
+      squared_dist += std::pow(point.y-max_pt.y(), 2);
+    if(point.z < min_pt.z())
+      squared_dist += std::pow(point.z-min_pt.z(), 2);
+    else if(point.z > max_pt.z())
+      squared_dist += std::pow(point.z-max_pt.z(), 2);
+    if (squared_dist < (radiusSquared + this->epsilon_)) {
       if (child_node->getNodeType() == BRANCH_NODE) {
         // we have not reached maximum tree depth
         getNeighborsWithinRadiusRecursive(point,