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// Copyright (C) 2013, OpenCV Foundation, all rights reserved. 16// Copyright (C) 2014, Itseez Inc, all rights reserved. 17// Third party copyrights are property of their respective owners. 18// 19// Redistribution and use in source and binary forms, with or without modification, 20// are permitted provided that the following conditions are met: 21// 22// * Redistribution's of source code must retain the above copyright notice, 23// this list of conditions and the following disclaimer. 24// 25// * Redistribution's in binary form must reproduce the above copyright notice, 26// this list of conditions and the following disclaimer in the documentation 27// and/or other materials provided with the distribution. 28// 29// * The name of the copyright holders may not be used to endorse or promote products 30// derived from this software without specific prior written permission. 31// 32// This software is provided by the copyright holders and contributors "as is" and 33// any express or implied warranties, including, but not limited to, the implied 34// warranties of merchantability and fitness for a particular purpose are disclaimed. 35// In no event shall the Intel Corporation or contributors be liable for any direct, 36// indirect, incidental, special, exemplary, or consequential damages 37// (including, but not limited to, procurement of substitute goods or services; 38// loss of use, data, or profits; or business interruption) however caused 39// and on any theory of liability, whether in contract, strict liability, 40// or tort (including negligence or otherwise) arising in any way out of 41// the use of this software, even if advised of the possibility of such damage. 42// 43//M*/ 44 45#include "precomp.hpp" 46#include "kdtree.hpp" 47 48namespace cv 49{ 50namespace ml 51{ 52// This is reimplementation of kd-trees from cvkdtree*.* by Xavier Delacour, cleaned-up and 53// adopted to work with the new OpenCV data structures. 54 55// The algorithm is taken from: 56// J.S. Beis and D.G. Lowe. Shape indexing using approximate nearest-neighbor search 57// in highdimensional spaces. In Proc. IEEE Conf. Comp. Vision Patt. Recog., 58// pages 1000--1006, 1997. http://citeseer.ist.psu.edu/beis97shape.html 59 60const int MAX_TREE_DEPTH = 32; 61 62KDTree::KDTree() 63{ 64 maxDepth = -1; 65 normType = NORM_L2; 66} 67 68KDTree::KDTree(InputArray _points, bool _copyData) 69{ 70 maxDepth = -1; 71 normType = NORM_L2; 72 build(_points, _copyData); 73} 74 75KDTree::KDTree(InputArray _points, InputArray _labels, bool _copyData) 76{ 77 maxDepth = -1; 78 normType = NORM_L2; 79 build(_points, _labels, _copyData); 80} 81 82struct SubTree 83{ 84 SubTree() : first(0), last(0), nodeIdx(0), depth(0) {} 85 SubTree(int _first, int _last, int _nodeIdx, int _depth) 86 : first(_first), last(_last), nodeIdx(_nodeIdx), depth(_depth) {} 87 int first; 88 int last; 89 int nodeIdx; 90 int depth; 91}; 92 93 94static float 95medianPartition( size_t* ofs, int a, int b, const float* vals ) 96{ 97 int k, a0 = a, b0 = b; 98 int middle = (a + b)/2; 99 while( b > a ) 100 { 101 int i0 = a, i1 = (a+b)/2, i2 = b; 102 float v0 = vals[ofs[i0]], v1 = vals[ofs[i1]], v2 = vals[ofs[i2]]; 103 int ip = v0 < v1 ? (v1 < v2 ? i1 : v0 < v2 ? i2 : i0) : 104 v0 < v2 ? i0 : (v1 < v2 ? i2 : i1); 105 float pivot = vals[ofs[ip]]; 106 std::swap(ofs[ip], ofs[i2]); 107 108 for( i1 = i0, i0--; i1 <= i2; i1++ ) 109 if( vals[ofs[i1]] <= pivot ) 110 { 111 i0++; 112 std::swap(ofs[i0], ofs[i1]); 113 } 114 if( i0 == middle ) 115 break; 116 if( i0 > middle ) 117 b = i0 - (b == i0); 118 else 119 a = i0; 120 } 121 122 float pivot = vals[ofs[middle]]; 123 int less = 0, more = 0; 124 for( k = a0; k < middle; k++ ) 125 { 126 CV_Assert(vals[ofs[k]] <= pivot); 127 less += vals[ofs[k]] < pivot; 128 } 129 for( k = b0; k > middle; k-- ) 130 { 131 CV_Assert(vals[ofs[k]] >= pivot); 132 more += vals[ofs[k]] > pivot; 133 } 134 CV_Assert(std::abs(more - less) <= 1); 135 136 return vals[ofs[middle]]; 137} 138 139static void 140computeSums( const Mat& points, const size_t* ofs, int a, int b, double* sums ) 141{ 142 int i, j, dims = points.cols; 143 const float* data = points.ptr<float>(0); 144 for( j = 0; j < dims; j++ ) 145 sums[j*2] = sums[j*2+1] = 0; 146 for( i = a; i <= b; i++ ) 147 { 148 const float* row = data + ofs[i]; 149 for( j = 0; j < dims; j++ ) 150 { 151 double t = row[j], s = sums[j*2] + t, s2 = sums[j*2+1] + t*t; 152 sums[j*2] = s; sums[j*2+1] = s2; 153 } 154 } 155} 156 157 158void KDTree::build(InputArray _points, bool _copyData) 159{ 160 build(_points, noArray(), _copyData); 161} 162 163 164void KDTree::build(InputArray __points, InputArray __labels, bool _copyData) 165{ 166 Mat _points = __points.getMat(), _labels = __labels.getMat(); 167 CV_Assert(_points.type() == CV_32F && !_points.empty()); 168 std::vector<KDTree::Node>().swap(nodes); 169 170 if( !_copyData ) 171 points = _points; 172 else 173 { 174 points.release(); 175 points.create(_points.size(), _points.type()); 176 } 177 178 int i, j, n = _points.rows, ptdims = _points.cols, top = 0; 179 const float* data = _points.ptr<float>(0); 180 float* dstdata = points.ptr<float>(0); 181 size_t step = _points.step1(); 182 size_t dstep = points.step1(); 183 int ptpos = 0; 184 labels.resize(n); 185 const int* _labels_data = 0; 186 187 if( !_labels.empty() ) 188 { 189 int nlabels = _labels.checkVector(1, CV_32S, true); 190 CV_Assert(nlabels == n); 191 _labels_data = _labels.ptr<int>(); 192 } 193 194 Mat sumstack(MAX_TREE_DEPTH*2, ptdims*2, CV_64F); 195 SubTree stack[MAX_TREE_DEPTH*2]; 196 197 std::vector<size_t> _ptofs(n); 198 size_t* ptofs = &_ptofs[0]; 199 200 for( i = 0; i < n; i++ ) 201 ptofs[i] = i*step; 202 203 nodes.push_back(Node()); 204 computeSums(points, ptofs, 0, n-1, sumstack.ptr<double>(top)); 205 stack[top++] = SubTree(0, n-1, 0, 0); 206 int _maxDepth = 0; 207 208 while( --top >= 0 ) 209 { 210 int first = stack[top].first, last = stack[top].last; 211 int depth = stack[top].depth, nidx = stack[top].nodeIdx; 212 int count = last - first + 1, dim = -1; 213 const double* sums = sumstack.ptr<double>(top); 214 double invCount = 1./count, maxVar = -1.; 215 216 if( count == 1 ) 217 { 218 int idx0 = (int)(ptofs[first]/step); 219 int idx = _copyData ? ptpos++ : idx0; 220 nodes[nidx].idx = ~idx; 221 if( _copyData ) 222 { 223 const float* src = data + ptofs[first]; 224 float* dst = dstdata + idx*dstep; 225 for( j = 0; j < ptdims; j++ ) 226 dst[j] = src[j]; 227 } 228 labels[idx] = _labels_data ? _labels_data[idx0] : idx0; 229 _maxDepth = std::max(_maxDepth, depth); 230 continue; 231 } 232 233 // find the dimensionality with the biggest variance 234 for( j = 0; j < ptdims; j++ ) 235 { 236 double m = sums[j*2]*invCount; 237 double varj = sums[j*2+1]*invCount - m*m; 238 if( maxVar < varj ) 239 { 240 maxVar = varj; 241 dim = j; 242 } 243 } 244 245 int left = (int)nodes.size(), right = left + 1; 246 nodes.push_back(Node()); 247 nodes.push_back(Node()); 248 nodes[nidx].idx = dim; 249 nodes[nidx].left = left; 250 nodes[nidx].right = right; 251 nodes[nidx].boundary = medianPartition(ptofs, first, last, data + dim); 252 253 int middle = (first + last)/2; 254 double *lsums = (double*)sums, *rsums = lsums + ptdims*2; 255 computeSums(points, ptofs, middle+1, last, rsums); 256 for( j = 0; j < ptdims*2; j++ ) 257 lsums[j] = sums[j] - rsums[j]; 258 stack[top++] = SubTree(first, middle, left, depth+1); 259 stack[top++] = SubTree(middle+1, last, right, depth+1); 260 } 261 maxDepth = _maxDepth; 262} 263 264 265struct PQueueElem 266{ 267 PQueueElem() : dist(0), idx(0) {} 268 PQueueElem(float _dist, int _idx) : dist(_dist), idx(_idx) {} 269 float dist; 270 int idx; 271}; 272 273 274int KDTree::findNearest(InputArray _vec, int K, int emax, 275 OutputArray _neighborsIdx, OutputArray _neighbors, 276 OutputArray _dist, OutputArray _labels) const 277 278{ 279 Mat vecmat = _vec.getMat(); 280 CV_Assert( vecmat.isContinuous() && vecmat.type() == CV_32F && vecmat.total() == (size_t)points.cols ); 281 const float* vec = vecmat.ptr<float>(); 282 K = std::min(K, points.rows); 283 int ptdims = points.cols; 284 285 CV_Assert(K > 0 && (normType == NORM_L2 || normType == NORM_L1)); 286 287 AutoBuffer<uchar> _buf((K+1)*(sizeof(float) + sizeof(int))); 288 int* idx = (int*)(uchar*)_buf; 289 float* dist = (float*)(idx + K + 1); 290 int i, j, ncount = 0, e = 0; 291 292 int qsize = 0, maxqsize = 1 << 10; 293 AutoBuffer<uchar> _pqueue(maxqsize*sizeof(PQueueElem)); 294 PQueueElem* pqueue = (PQueueElem*)(uchar*)_pqueue; 295 emax = std::max(emax, 1); 296 297 for( e = 0; e < emax; ) 298 { 299 float d, alt_d = 0.f; 300 int nidx; 301 302 if( e == 0 ) 303 nidx = 0; 304 else 305 { 306 // take the next node from the priority queue 307 if( qsize == 0 ) 308 break; 309 nidx = pqueue[0].idx; 310 alt_d = pqueue[0].dist; 311 if( --qsize > 0 ) 312 { 313 std::swap(pqueue[0], pqueue[qsize]); 314 d = pqueue[0].dist; 315 for( i = 0;;) 316 { 317 int left = i*2 + 1, right = i*2 + 2; 318 if( left >= qsize ) 319 break; 320 if( right < qsize && pqueue[right].dist < pqueue[left].dist ) 321 left = right; 322 if( pqueue[left].dist >= d ) 323 break; 324 std::swap(pqueue[i], pqueue[left]); 325 i = left; 326 } 327 } 328 329 if( ncount == K && alt_d > dist[ncount-1] ) 330 continue; 331 } 332 333 for(;;) 334 { 335 if( nidx < 0 ) 336 break; 337 const Node& n = nodes[nidx]; 338 339 if( n.idx < 0 ) 340 { 341 i = ~n.idx; 342 const float* row = points.ptr<float>(i); 343 if( normType == NORM_L2 ) 344 for( j = 0, d = 0.f; j < ptdims; j++ ) 345 { 346 float t = vec[j] - row[j]; 347 d += t*t; 348 } 349 else 350 for( j = 0, d = 0.f; j < ptdims; j++ ) 351 d += std::abs(vec[j] - row[j]); 352 353 dist[ncount] = d; 354 idx[ncount] = i; 355 for( i = ncount-1; i >= 0; i-- ) 356 { 357 if( dist[i] <= d ) 358 break; 359 std::swap(dist[i], dist[i+1]); 360 std::swap(idx[i], idx[i+1]); 361 } 362 ncount += ncount < K; 363 e++; 364 break; 365 } 366 367 int alt; 368 if( vec[n.idx] <= n.boundary ) 369 { 370 nidx = n.left; 371 alt = n.right; 372 } 373 else 374 { 375 nidx = n.right; 376 alt = n.left; 377 } 378 379 d = vec[n.idx] - n.boundary; 380 if( normType == NORM_L2 ) 381 d = d*d + alt_d; 382 else 383 d = std::abs(d) + alt_d; 384 // subtree prunning 385 if( ncount == K && d > dist[ncount-1] ) 386 continue; 387 // add alternative subtree to the priority queue 388 pqueue[qsize] = PQueueElem(d, alt); 389 for( i = qsize; i > 0; ) 390 { 391 int parent = (i-1)/2; 392 if( parent < 0 || pqueue[parent].dist <= d ) 393 break; 394 std::swap(pqueue[i], pqueue[parent]); 395 i = parent; 396 } 397 qsize += qsize+1 < maxqsize; 398 } 399 } 400 401 K = std::min(K, ncount); 402 if( _neighborsIdx.needed() ) 403 { 404 _neighborsIdx.create(K, 1, CV_32S, -1, true); 405 Mat nidx = _neighborsIdx.getMat(); 406 Mat(nidx.size(), CV_32S, &idx[0]).copyTo(nidx); 407 } 408 if( _dist.needed() ) 409 sqrt(Mat(K, 1, CV_32F, dist), _dist); 410 411 if( _neighbors.needed() || _labels.needed() ) 412 getPoints(Mat(K, 1, CV_32S, idx), _neighbors, _labels); 413 return K; 414} 415 416 417void KDTree::findOrthoRange(InputArray _lowerBound, 418 InputArray _upperBound, 419 OutputArray _neighborsIdx, 420 OutputArray _neighbors, 421 OutputArray _labels ) const 422{ 423 int ptdims = points.cols; 424 Mat lowerBound = _lowerBound.getMat(), upperBound = _upperBound.getMat(); 425 CV_Assert( lowerBound.size == upperBound.size && 426 lowerBound.isContinuous() && 427 upperBound.isContinuous() && 428 lowerBound.type() == upperBound.type() && 429 lowerBound.type() == CV_32F && 430 lowerBound.total() == (size_t)ptdims ); 431 const float* L = lowerBound.ptr<float>(); 432 const float* R = upperBound.ptr<float>(); 433 434 std::vector<int> idx; 435 AutoBuffer<int> _stack(MAX_TREE_DEPTH*2 + 1); 436 int* stack = _stack; 437 int top = 0; 438 439 stack[top++] = 0; 440 441 while( --top >= 0 ) 442 { 443 int nidx = stack[top]; 444 if( nidx < 0 ) 445 break; 446 const Node& n = nodes[nidx]; 447 if( n.idx < 0 ) 448 { 449 int j, i = ~n.idx; 450 const float* row = points.ptr<float>(i); 451 for( j = 0; j < ptdims; j++ ) 452 if( row[j] < L[j] || row[j] >= R[j] ) 453 break; 454 if( j == ptdims ) 455 idx.push_back(i); 456 continue; 457 } 458 if( L[n.idx] <= n.boundary ) 459 stack[top++] = n.left; 460 if( R[n.idx] > n.boundary ) 461 stack[top++] = n.right; 462 } 463 464 if( _neighborsIdx.needed() ) 465 { 466 _neighborsIdx.create((int)idx.size(), 1, CV_32S, -1, true); 467 Mat nidx = _neighborsIdx.getMat(); 468 Mat(nidx.size(), CV_32S, &idx[0]).copyTo(nidx); 469 } 470 getPoints( idx, _neighbors, _labels ); 471} 472 473 474void KDTree::getPoints(InputArray _idx, OutputArray _pts, OutputArray _labels) const 475{ 476 Mat idxmat = _idx.getMat(), pts, labelsmat; 477 CV_Assert( idxmat.isContinuous() && idxmat.type() == CV_32S && 478 (idxmat.cols == 1 || idxmat.rows == 1) ); 479 const int* idx = idxmat.ptr<int>(); 480 int* dstlabels = 0; 481 482 int ptdims = points.cols; 483 int i, nidx = (int)idxmat.total(); 484 if( nidx == 0 ) 485 { 486 _pts.release(); 487 _labels.release(); 488 return; 489 } 490 491 if( _pts.needed() ) 492 { 493 _pts.create( nidx, ptdims, points.type()); 494 pts = _pts.getMat(); 495 } 496 497 if(_labels.needed()) 498 { 499 _labels.create(nidx, 1, CV_32S, -1, true); 500 labelsmat = _labels.getMat(); 501 CV_Assert( labelsmat.isContinuous() ); 502 dstlabels = labelsmat.ptr<int>(); 503 } 504 const int* srclabels = !labels.empty() ? &labels[0] : 0; 505 506 for( i = 0; i < nidx; i++ ) 507 { 508 int k = idx[i]; 509 CV_Assert( (unsigned)k < (unsigned)points.rows ); 510 const float* src = points.ptr<float>(k); 511 if( !pts.empty() ) 512 std::copy(src, src + ptdims, pts.ptr<float>(i)); 513 if( dstlabels ) 514 dstlabels[i] = srclabels ? srclabels[k] : k; 515 } 516} 517 518 519const float* KDTree::getPoint(int ptidx, int* label) const 520{ 521 CV_Assert( (unsigned)ptidx < (unsigned)points.rows); 522 if(label) 523 *label = labels[ptidx]; 524 return points.ptr<float>(ptidx); 525} 526 527 528int KDTree::dims() const 529{ 530 return !points.empty() ? points.cols : 0; 531} 532 533} 534} 535