| 3897 | |
| 3898 | |
| 3899 | void KDTree::findOrthoRange(InputArray _lowerBound, |
| 3900 | InputArray _upperBound, |
| 3901 | OutputArray _neighborsIdx, |
| 3902 | OutputArray _neighbors, |
| 3903 | OutputArray _labels ) const |
| 3904 | { |
| 3905 | int ptdims = points.cols; |
| 3906 | Mat lowerBound = _lowerBound.getMat(), upperBound = _upperBound.getMat(); |
| 3907 | CV_Assert( lowerBound.size == upperBound.size && |
| 3908 | lowerBound.isContinuous() && |
| 3909 | upperBound.isContinuous() && |
| 3910 | lowerBound.type() == upperBound.type() && |
| 3911 | lowerBound.type() == CV_32F && |
| 3912 | lowerBound.total() == (size_t)ptdims ); |
| 3913 | const float* L = lowerBound.ptr<float>(); |
| 3914 | const float* R = upperBound.ptr<float>(); |
| 3915 | |
| 3916 | vector<int> idx; |
| 3917 | AutoBuffer<int> _stack(MAX_TREE_DEPTH*2 + 1); |
| 3918 | int* stack = _stack; |
| 3919 | int top = 0; |
| 3920 | |
| 3921 | stack[top++] = 0; |
| 3922 | |
| 3923 | while( --top >= 0 ) |
| 3924 | { |
| 3925 | int nidx = stack[top]; |
| 3926 | if( nidx < 0 ) |
| 3927 | break; |
| 3928 | const Node& n = nodes[nidx]; |
| 3929 | if( n.idx < 0 ) |
| 3930 | { |
| 3931 | int j, i = ~n.idx; |
| 3932 | const float* row = points.ptr<float>(i); |
| 3933 | for( j = 0; j < ptdims; j++ ) |
| 3934 | if( row[j] < L[j] || row[j] >= R[j] ) |
| 3935 | break; |
| 3936 | if( j == ptdims ) |
| 3937 | idx.push_back(i); |
| 3938 | continue; |
| 3939 | } |
| 3940 | if( L[n.idx] <= n.boundary ) |
| 3941 | stack[top++] = n.left; |
| 3942 | if( R[n.idx] > n.boundary ) |
| 3943 | stack[top++] = n.right; |
| 3944 | } |
| 3945 | |
| 3946 | if( _neighborsIdx.needed() ) |
| 3947 | { |
| 3948 | _neighborsIdx.create((int)idx.size(), 1, CV_32S, -1, true); |
| 3949 | Mat nidx = _neighborsIdx.getMat(); |
| 3950 | Mat(nidx.size(), CV_32S, &idx[0]).copyTo(nidx); |
| 3951 | } |
| 3952 | getPoints( idx, _neighbors, _labels ); |
| 3953 | } |
| 3954 | |
| 3955 | |
| 3956 | void KDTree::getPoints(InputArray _idx, OutputArray _pts, OutputArray _labels) const |