modules/contrib/src/octree.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                           License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15 // Third party copyrights are property of their respective owners.
  16 //
  17 // Redistribution and use in source and binary forms, with or without modification,
  18 // are permitted provided that the following conditions are met:
  19 //
  20 //   * Redistribution's of source code must retain the above copyright notice,
  21 //     this list of conditions and the following disclaimer.
  22 //
  23 //   * Redistribution's in binary form must reproduce the above copyright notice,
  24 //     this list of conditions and the following disclaimer in the documentation
  25 //     and/or other materials provided with the distribution.
  26 //
  27 //   * The name of the copyright holders may not be used to endorse or promote products
  28 //     derived from this software without specific prior written permission.
  29 //
  30 // This software is provided by the copyright holders and contributors "as is" and
  31 // any express or implied warranties, including, but not limited to, the implied
  32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33 // In no event shall the Intel Corporation or contributors be liable for any direct,
  34 // indirect, incidental, special, exemplary, or consequential damages
  35 // (including, but not limited to, procurement of substitute goods or services;
  36 // loss of use, data, or profits; or business interruption) however caused
  37 // and on any theory of liability, whether in contract, strict liability,
  38 // or tort (including negligence or otherwise) arising in any way out of
  39 // the use of this software, even if advised of the possibility of such damage.
  40 //
  41 //M*/
  42
  43 #include "precomp.hpp"
  44 #include <limits>
  45
  46 namespace
  47 {
  48     using namespace cv;
  49     const size_t MAX_STACK_SIZE = 255;
  50     const size_t MAX_LEAFS = 8;
  51
  52     bool checkIfNodeOutsideSphere(const Octree::Node& node, const Point3f& c, float r)
  53     {
  54         if (node.x_max < (c.x - r) ||  node.y_max < (c.y - r) || node.z_max < (c.z - r))
  55             return true;
  56
  57         if ((c.x + r) < node.x_min || (c.y + r) < node.y_min || (c.z + r) < node.z_min)
  58             return true;
  59
  60         return false;
  61     }
  62
  63     bool checkIfNodeInsideSphere(const Octree::Node& node, const Point3f& c, float r)
  64     {
  65         r *= r;
  66
  67         float d2_xmin = (node.x_min - c.x) * (node.x_min - c.x);
  68         float d2_ymin = (node.y_min - c.y) * (node.y_min - c.y);
  69         float d2_zmin = (node.z_min - c.z) * (node.z_min - c.z);
  70
  71         if (d2_xmin + d2_ymin + d2_zmin > r)
  72             return false;
  73
  74         float d2_zmax = (node.z_max - c.z) * (node.z_max - c.z);
  75
  76         if (d2_xmin + d2_ymin + d2_zmax > r)
  77             return false;
  78
  79         float d2_ymax = (node.y_max - c.y) * (node.y_max - c.y);
  80
  81         if (d2_xmin + d2_ymax + d2_zmin > r)
  82             return false;
  83
  84         if (d2_xmin + d2_ymax + d2_zmax > r)
  85             return false;
  86
  87         float d2_xmax = (node.x_max - c.x) * (node.x_max - c.x);
  88
  89         if (d2_xmax + d2_ymin + d2_zmin > r)
  90             return false;
  91
  92         if (d2_xmax + d2_ymin + d2_zmax > r)
  93             return false;
  94
  95         if (d2_xmax + d2_ymax + d2_zmin > r)
  96             return false;
  97
  98         if (d2_xmax + d2_ymax + d2_zmax > r)
  99             return false;
 100
 101         return true;
 102     }
 103
 104     void fillMinMax(const vector<Point3f>& points, Octree::Node& node)
 105     {
 106         node.x_max = node.y_max = node.z_max = std::numeric_limits<float>::min();
 107         node.x_min = node.y_min = node.z_min = std::numeric_limits<float>::max();
 108
 109         for (size_t i = 0; i < points.size(); ++i)
 110         {
 111             const Point3f& point = points[i];
 112
 113             if (node.x_max < point.x)
 114                 node.x_max = point.x;
 115
 116             if (node.y_max < point.y)
 117                 node.y_max = point.y;
 118
 119             if (node.z_max < point.z)
 120                 node.z_max = point.z;
 121
 122             if (node.x_min > point.x)
 123                 node.x_min = point.x;
 124
 125             if (node.y_min > point.y)
 126                 node.y_min = point.y;
 127
 128             if (node.z_min > point.z)
 129                 node.z_min = point.z;
 130         }
 131     }
 132
 133     size_t findSubboxForPoint(const Point3f& point, const Octree::Node& node)
 134     {
 135         size_t ind_x = point.x < (node.x_max + node.x_min) / 2 ? 0 : 1;
 136         size_t ind_y = point.y < (node.y_max + node.y_min) / 2 ? 0 : 1;
 137         size_t ind_z = point.z < (node.z_max + node.z_min) / 2 ? 0 : 1;
 138
 139         return (ind_x << 2) + (ind_y << 1) + (ind_z << 0);
 140     }
 141     void initChildBox(const Octree::Node& parent, size_t boxIndex, Octree::Node& child)
 142     {
 143         child.x_min = child.x_max = (parent.x_max + parent.x_min) / 2;
 144         child.y_min = child.y_max = (parent.y_max + parent.y_min) / 2;
 145         child.z_min = child.z_max = (parent.z_max + parent.z_min) / 2;
 146
 147         if ((boxIndex >> 0) & 1)
 148             child.z_max = parent.z_max;
 149         else
 150             child.z_min = parent.z_min;
 151
 152         if ((boxIndex >> 1) & 1)
 153             child.y_max = parent.y_max;
 154         else
 155             child.y_min = parent.y_min;
 156
 157         if ((boxIndex >> 2) & 1)
 158             child.x_max = parent.x_max;
 159         else
 160             child.x_min = parent.x_min;
 161     }
 162
 163 }//namespace
 164
 165 ////////////////////////////////////////////////////////////////////////////////////////
 166 ///////////////////////////       Octree       //////////////////////////////////////
 167 ////////////////////////////////////////////////////////////////////////////////////////
 168 namespace cv
 169 {
 170     Octree::Octree()
 171     {
 172     }
 173
 174     Octree::Octree(const vector<Point3f>& points3d, int maxLevels, int _minPoints)
 175     {
 176         buildTree(points3d, maxLevels, _minPoints);
 177     }
 178
 179     Octree::~Octree()
 180     {
 181     }
 182
 183     void Octree::getPointsWithinSphere(const Point3f& center, float radius, vector<Point3f>& out) const
 184     {
 185         out.clear();
 186
 187         if (nodes.empty())
 188             return;
 189
 190         int stack[MAX_STACK_SIZE];
 191         int pos = 0;
 192         stack[pos] = 0;
 193
 194         while (pos >= 0)
 195         {
 196             const Node& cur = nodes[stack[pos--]];
 197
 198             if (checkIfNodeOutsideSphere(cur, center, radius))
 199                 continue;
 200
 201             if (checkIfNodeInsideSphere(cur, center, radius))
 202             {
 203                 size_t sz = out.size();
 204                 out.resize(sz + cur.end - cur.begin);
 205                 for (int i = cur.begin; i < cur.end; ++i)
 206                     out[sz++] = points[i];
 207                 continue;
 208             }
 209
 210             if (cur.isLeaf)
 211             {
 212                 double r2 = radius * radius;
 213                 size_t sz = out.size();
 214                 out.resize(sz + (cur.end - cur.begin));
 215
 216                 for (int i = cur.begin; i < cur.end; ++i)
 217                 {
 218                     const Point3f& point = points[i];
 219
 220                     double dx = (point.x - center.x);
 221                     double dy = (point.y - center.y);
 222                     double dz = (point.z - center.z);
 223
 224                     double dist2 = dx * dx + dy * dy + dz * dz;
 225
 226                     if (dist2 < r2)
 227                         out[sz++] = point;
 228                 };
 229                 out.resize(sz);
 230                 continue;
 231             }
 232
 233             if (cur.children[0])
 234                 stack[++pos] = cur.children[0];
 235
 236             if (cur.children[1])
 237                 stack[++pos] = cur.children[1];
 238
 239             if (cur.children[2])
 240                 stack[++pos] = cur.children[2];
 241
 242             if (cur.children[3])
 243                 stack[++pos] = cur.children[3];
 244
 245             if (cur.children[4])
 246                 stack[++pos] = cur.children[4];
 247
 248             if (cur.children[5])
 249                 stack[++pos] = cur.children[5];
 250
 251             if (cur.children[6])
 252                 stack[++pos] = cur.children[6];
 253
 254             if (cur.children[7])
 255                 stack[++pos] = cur.children[7];
 256         }
 257     }
 258
 259     void Octree::buildTree(const vector<Point3f>& points3d, int maxLevels, int _minPoints)
 260     {
 261         assert((size_t)maxLevels * 8 < MAX_STACK_SIZE);
 262         points.resize(points3d.size());
 263         std::copy(points3d.begin(), points3d.end(), points.begin());
 264         minPoints = _minPoints;
 265
 266         nodes.clear();
 267         nodes.push_back(Node());
 268         Node& root = nodes[0];
 269         fillMinMax(points, root);
 270
 271         root.isLeaf = true;
 272         root.maxLevels = maxLevels;
 273         root.begin = 0;
 274         root.end = (int)points.size();
 275         for (size_t i = 0; i < MAX_LEAFS; i++)
 276             root.children[i] = 0;
 277
 278         if (maxLevels != 1 && (root.end - root.begin) > _minPoints)
 279         {
 280             root.isLeaf = false;
 281             buildNext(0);
 282         }
 283     }
 284
 285     void  Octree::buildNext(size_t nodeInd)
 286     {
 287         size_t size = nodes[nodeInd].end - nodes[nodeInd].begin;
 288
 289         vector<size_t> boxBorders(MAX_LEAFS+1, 0);
 290         vector<size_t> boxIndices(size);
 291         vector<Point3f> tempPoints(size);
 292
 293         for (int i = nodes[nodeInd].begin, j = 0; i < nodes[nodeInd].end; ++i, ++j)
 294         {
 295             const Point3f& p = points[i];
 296
 297             size_t subboxInd = findSubboxForPoint(p, nodes[nodeInd]);
 298
 299             boxBorders[subboxInd+1]++;
 300             boxIndices[j] = subboxInd;
 301             tempPoints[j] = p;
 302         }
 303
 304         for (size_t i = 1; i < boxBorders.size(); ++i)
 305             boxBorders[i] += boxBorders[i-1];
 306
 307         vector<size_t> writeInds(boxBorders.begin(), boxBorders.end());
 308
 309         for (size_t i = 0; i < size; ++i)
 310         {
 311             size_t boxIndex = boxIndices[i];
 312             Point3f& curPoint = tempPoints[i];
 313
 314             size_t copyTo = nodes[nodeInd].begin + writeInds[boxIndex]++;
 315             points[copyTo] = curPoint;
 316         }
 317
 318         for (size_t i = 0; i < MAX_LEAFS; ++i)
 319         {
 320             if (boxBorders[i] == boxBorders[i+1])
 321                 continue;
 322
 323             nodes.push_back(Node());
 324             Node& child = nodes.back();
 325             initChildBox(nodes[nodeInd], i, child);
 326
 327             child.isLeaf = true;
 328             child.maxLevels = nodes[nodeInd].maxLevels - 1;
 329             child.begin = nodes[nodeInd].begin + (int)boxBorders[i+0];
 330             child.end   = nodes[nodeInd].begin + (int)boxBorders[i+1];
 331             for (size_t k = 0; k < MAX_LEAFS; k++)
 332                 child.children[k] = 0;
 333
 334             nodes[nodeInd].children[i] = (int)(nodes.size() - 1);
 335
 336             if (child.maxLevels != 1 && (child.end - child.begin) > minPoints)
 337             {
 338                 child.isLeaf = false;
 339                 buildNext(nodes.size() - 1);
 340             }
 341         }
 342     }
 343
 344 }