3dpcp/.svn/pristine/37/37cc9106c27a8cfda8e1ad902242e9f14314cb6b.svn-base
2012-09-16 14:33:11 +02:00

183 lines
7.2 KiB
Text

//----------------------------------------------------------------------
// File: kd_fix_rad_search.cpp
// Programmer: Sunil Arya and David Mount
// Description: Standard kd-tree fixed-radius kNN search
// Last modified: 05/03/05 (Version 1.1)
//----------------------------------------------------------------------
// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and
// David Mount. All Rights Reserved.
//
// This software and related documentation is part of the Approximate
// Nearest Neighbor Library (ANN). This software is provided under
// the provisions of the Lesser GNU Public License (LGPL). See the
// file ../ReadMe.txt for further information.
//
// The University of Maryland (U.M.) and the authors make no
// representations about the suitability or fitness of this software for
// any purpose. It is provided "as is" without express or implied
// warranty.
//----------------------------------------------------------------------
// History:
// Revision 1.1 05/03/05
// Initial release
//----------------------------------------------------------------------
#include "kd_fix_rad_search.h" // kd fixed-radius search decls
//----------------------------------------------------------------------
// Approximate fixed-radius k nearest neighbor search
// The squared radius is provided, and this procedure finds the
// k nearest neighbors within the radius, and returns the total
// number of points lying within the radius.
//
// The method used for searching the kd-tree is a variation of the
// nearest neighbor search used in kd_search.cpp, except that the
// radius of the search ball is known. We refer the reader to that
// file for the explanation of the recursive search procedure.
//----------------------------------------------------------------------
//----------------------------------------------------------------------
// To keep argument lists short, a number of global variables
// are maintained which are common to all the recursive calls.
// These are given below.
//----------------------------------------------------------------------
int ANNkdFRDim; // dimension of space
ANNpoint ANNkdFRQ; // query point
ANNdist ANNkdFRSqRad; // squared radius search bound
double ANNkdFRMaxErr; // max tolerable squared error
ANNpointArray ANNkdFRPts; // the points
ANNmin_k* ANNkdFRPointMK; // set of k closest points
int ANNkdFRPtsVisited; // total points visited
int ANNkdFRPtsInRange; // number of points in the range
//----------------------------------------------------------------------
// annkFRSearch - fixed radius search for k nearest neighbors
//----------------------------------------------------------------------
int ANNkd_tree::annkFRSearch(
ANNpoint q, // the query point
ANNdist sqRad, // squared radius search bound
int k, // number of near neighbors to return
ANNidxArray nn_idx, // nearest neighbor indices (returned)
ANNdistArray dd, // the approximate nearest neighbor
double eps) // the error bound
{
ANNkdFRDim = dim; // copy arguments to static equivs
ANNkdFRQ = q;
ANNkdFRSqRad = sqRad;
ANNkdFRPts = pts;
ANNkdFRPtsVisited = 0; // initialize count of points visited
ANNkdFRPtsInRange = 0; // ...and points in the range
ANNkdFRMaxErr = ANN_POW(1.0 + eps);
ANN_FLOP(2) // increment floating op count
ANNkdFRPointMK = new ANNmin_k(k); // create set for closest k points
// search starting at the root
root->ann_FR_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim));
for (int i = 0; i < k; i++) { // extract the k-th closest points
if (dd != NULL)
dd[i] = ANNkdFRPointMK->ith_smallest_key(i);
if (nn_idx != NULL)
nn_idx[i] = ANNkdFRPointMK->ith_smallest_info(i);
}
delete ANNkdFRPointMK; // deallocate closest point set
return ANNkdFRPtsInRange; // return final point count
}
//----------------------------------------------------------------------
// kd_split::ann_FR_search - search a splitting node
// Note: This routine is similar in structure to the standard kNN
// search. It visits the subtree that is closer to the query point
// first. For fixed-radius search, there is no benefit in visiting
// one subtree before the other, but we maintain the same basic
// code structure for the sake of uniformity.
//----------------------------------------------------------------------
void ANNkd_split::ann_FR_search(ANNdist box_dist)
{
// check dist calc term condition
if (ANNmaxPtsVisited != 0 && ANNkdFRPtsVisited > ANNmaxPtsVisited) return;
// distance to cutting plane
ANNcoord cut_diff = ANNkdFRQ[cut_dim] - cut_val;
if (cut_diff < 0) { // left of cutting plane
child[ANN_LO]->ann_FR_search(box_dist);// visit closer child first
ANNcoord box_diff = cd_bnds[ANN_LO] - ANNkdFRQ[cut_dim];
if (box_diff < 0) // within bounds - ignore
box_diff = 0;
// distance to further box
box_dist = (ANNdist) ANN_SUM(box_dist,
ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));
// visit further child if in range
if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)
child[ANN_HI]->ann_FR_search(box_dist);
}
else { // right of cutting plane
child[ANN_HI]->ann_FR_search(box_dist);// visit closer child first
ANNcoord box_diff = ANNkdFRQ[cut_dim] - cd_bnds[ANN_HI];
if (box_diff < 0) // within bounds - ignore
box_diff = 0;
// distance to further box
box_dist = (ANNdist) ANN_SUM(box_dist,
ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff)));
// visit further child if close enough
if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)
child[ANN_LO]->ann_FR_search(box_dist);
}
ANN_FLOP(13) // increment floating ops
ANN_SPL(1) // one more splitting node visited
}
//----------------------------------------------------------------------
// kd_leaf::ann_FR_search - search points in a leaf node
// Note: The unreadability of this code is the result of
// some fine tuning to replace indexing by pointer operations.
//----------------------------------------------------------------------
void ANNkd_leaf::ann_FR_search(ANNdist box_dist)
{
register ANNdist dist; // distance to data point
register ANNcoord* pp; // data coordinate pointer
register ANNcoord* qq; // query coordinate pointer
register ANNcoord t;
register int d;
for (int i = 0; i < n_pts; i++) { // check points in bucket
pp = ANNkdFRPts[bkt[i]]; // first coord of next data point
qq = ANNkdFRQ; // first coord of query point
dist = 0;
for(d = 0; d < ANNkdFRDim; d++) {
ANN_COORD(1) // one more coordinate hit
ANN_FLOP(5) // increment floating ops
t = *(qq++) - *(pp++); // compute length and adv coordinate
// exceeds dist to k-th smallest?
if( (dist = ANN_SUM(dist, ANN_POW(t))) > ANNkdFRSqRad) {
break;
}
}
if (d >= ANNkdFRDim && // among the k best?
(ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem
// add it to the list
ANNkdFRPointMK->insert(dist, bkt[i]);
ANNkdFRPtsInRange++; // increment point count
}
}
ANN_LEAF(1) // one more leaf node visited
ANN_PTS(n_pts) // increment points visited
ANNkdFRPtsVisited += n_pts; // increment number of points visited
}