vcs.maemo.org Git - opencv/blob - src/cvaux/cvoctree.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 "_cvaux.h"
  44 #include "cxmisc.h"
  45 #include "limits"
  46
  47 namespace cv
  48 {
  49
  50 const size_t MAX_STACK_SIZE = 255;
  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& perent, size_t boxIndex, Octree::Node& child)
 142 {
 143     child.x_min = child.x_max = (perent.x_max + perent.x_min) / 2;
 144     child.y_min = child.y_max = (perent.y_max + perent.y_min) / 2;
 145     child.z_min = child.z_max = (perent.z_max + perent.z_min) / 2;
 146
 147     if ((boxIndex >> 0) & 1)
 148         child.z_max = perent.z_max;
 149     else
 150         child.z_min = perent.z_min;
 151
 152     if ((boxIndex >> 1) & 1)
 153         child.y_max = perent.y_max;
 154     else
 155         child.y_min = perent.y_min;
 156
 157     if ((boxIndex >> 2) & 1)
 158         child.x_max = perent.x_max;
 159     else
 160         child.x_min = perent.x_min;
 161 }
 162
 163 ////////////////////////////////////////////////////////////////////////////////////////
 164 ///////////////////////////       Octree       //////////////////////////////////////
 165 ////////////////////////////////////////////////////////////////////////////////////////
 166
 167 Octree::Octree()
 168 {
 169 }
 170
 171 Octree::Octree( const vector<Point3f>& points3d, int maxLevels, int minPoints )
 172 {
 173     buildTree(points3d, maxLevels, minPoints);
 174 }
 175
 176 Octree::~Octree()
 177 {
 178 }
 179
 180 void Octree::getPointsWithinSphere( const Point3f& center, float radius, vector<Point3f>& out ) const
 181 {
 182     out.clear();
 183
 184     if( nodes.empty() )
 185         return;
 186
 187     int stack[MAX_STACK_SIZE];
 188     int pos = 0;
 189     stack[pos] = 0;
 190
 191     while(pos >= 0)
 192     {
 193         const Node& cur = nodes[stack[pos--]];
 194
 195         if (checkIfNodeOutsideSphere(cur, center, radius))
 196             continue;
 197
 198         if(checkIfNodeInsideSphere(cur, center, radius))
 199         {
 200             size_t sz = out.size();
 201             out.resize(sz + cur.end - cur.begin);
 202             for(int i = cur.begin; i < cur.end; ++i)
 203                 out[sz++] = points[i];;
 204             continue;
 205         }
 206
 207         if (cur.isLeaf)
 208         {
 209             double r2 = radius * radius;
 210             size_t sz = out.size();
 211             out.resize(sz + (cur.end - cur.begin));
 212
 213             for(int i = cur.begin; i < cur.end; ++i)
 214             {
 215                 const Point3f& point = points[i];
 216
 217                 double dx = (point.x - center.x);
 218                 double dy = (point.y - center.y);
 219                 double dz = (point.z - center.z);
 220
 221                 double dist2 = dx*dx + dy*dy + dz*dz;
 222
 223                 if (dist2 < r2)
 224                     out[sz++] = point;
 225             };
 226             out.resize(sz);
 227             continue;
 228         }
 229
 230         if (cur.children[0])
 231             stack[++pos] = cur.children[0];
 232
 233         if (cur.children[1])
 234             stack[++pos] = cur.children[1];
 235
 236         if (cur.children[2])
 237             stack[++pos] = cur.children[2];
 238
 239         if (cur.children[3])
 240             stack[++pos] = cur.children[3];
 241
 242         if (cur.children[4])
 243             stack[++pos] = cur.children[4];
 244
 245         if (cur.children[5])
 246             stack[++pos] = cur.children[5];
 247
 248         if (cur.children[6])
 249             stack[++pos] = cur.children[6];
 250
 251         if (cur.children[7])
 252             stack[++pos] = cur.children[7];
 253     }
 254 }
 255
 256 void Octree::buildTree( const vector<Point3f>& points3d, int maxLevels, int minPoints)
 257 {
 258     assert( (size_t)maxLevels * 8 < MAX_STACK_SIZE );
 259     points.resize(points3d.size());
 260     std::copy(points3d.begin(), points3d.end(), points.begin());
 261     this->minPoints = minPoints;
 262
 263     nodes.clear();
 264     nodes.push_back(Node());
 265     Node& root = nodes[0];
 266     fillMinMax(points, root);
 267
 268     root.isLeaf = true;
 269     root.maxLevels = maxLevels;
 270     root.begin = 0;
 271     root.end = (int)points.size();
 272     for( int i = 0; i < 8; i++ )
 273         root.children[i] = 0;
 274
 275     if (maxLevels != 1 && (root.end - root.begin) > minPoints)
 276     {
 277         root.isLeaf = false;
 278         buildNext(0);
 279     }
 280 }
 281
 282 void  Octree::buildNext(size_t node_ind)
 283 {
 284     size_t size = nodes[node_ind].end - nodes[node_ind].begin;
 285
 286     vector<size_t> boxBorders(9, 0);
 287     vector<size_t> boxIndeces(size);
 288     vector<Point3f> tempPoints(size);
 289
 290     for(int i = nodes[node_ind].begin, j = 0; i < nodes[node_ind].end; ++i, ++j)
 291     {
 292         const Point3f& p = points[i];
 293
 294         size_t subbox_ind = findSubboxForPoint(p, nodes[node_ind]);
 295
 296         boxBorders[subbox_ind+1]++;
 297         boxIndeces[j] = subbox_ind;
 298         tempPoints[j] = p;
 299     }
 300
 301     for(size_t i = 1; i < boxBorders.size(); ++i)
 302         boxBorders[i] += boxBorders[i-1];
 303
 304     vector<size_t> writeInds(boxBorders.begin(), boxBorders.end());
 305
 306     for(size_t i = 0; i < size; ++i)
 307     {
 308         size_t boxIndex = boxIndeces[i];
 309         Point3f& curPoint = tempPoints[i];
 310
 311         size_t copyTo = nodes[node_ind].begin + writeInds[boxIndex]++;
 312         points[copyTo] = curPoint;
 313     }
 314
 315     for(size_t i = 0; i < 8; ++i)
 316     {
 317         if (boxBorders[i] == boxBorders[i+1])
 318             continue;
 319
 320         nodes.push_back(Node());
 321         Node& child = nodes.back();
 322         initChildBox(nodes[node_ind], i, child);
 323
 324         child.isLeaf = true;
 325         child.maxLevels = nodes[node_ind].maxLevels - 1;
 326         child.begin = nodes[node_ind].begin + (int)boxBorders[i+0];
 327         child.end   = nodes[node_ind].begin + (int)boxBorders[i+1];
 328         for( int k = 0; k < 8; k++ )
 329             child.children[k] = 0;
 330
 331         nodes[node_ind].children[i] = (int)(nodes.size() - 1);
 332
 333         if (child.maxLevels != 1 && (child.end - child.begin) > minPoints )
 334         {
 335             child.isLeaf = false;
 336             buildNext(nodes.size() - 1);
 337         }
 338     }
 339 }
 340
 341 }