VL_EXPORT int
vl_kdforest_query_recursively (VlKDForest * self,
VlKDTree * tree,
vl_uindex nodeIndex,
VlKDForestNeighbor * neighbors,
vl_size numNeighbors,
vl_size * numAddedNeighbors,
double dist,
void const * query)
{
VlKDTreeNode const * node = tree->nodes + nodeIndex ;
vl_uindex i = node->splitDimension ;
vl_index nextChild, saveChild ;
double delta, saveDist ;
double x ;
double x1 = node->lowerBound ;
double x2 = node->splitThreshold ;
double x3 = node->upperBound ;
VlKDForestSearchState * searchState ;
self->searchNumRecursions ++ ;
switch (self->dataType) {
case VL_TYPE_FLOAT :
x = ((float const*) query)[i] ;
break ;
case VL_TYPE_DOUBLE :
x = ((double const*) query)[i] ;
break ;
default :
abort() ;
}
/* base case: this is a leaf node */
if (node->lowerChild < 0) {
vl_index begin = - node->lowerChild - 1 ;
vl_index end = - node->upperChild - 1 ;
vl_index iter ;
for (iter = begin ;
iter < end &&
(self->searchMaxNumComparisons == 0 ||
self->searchNumComparisons < self->searchMaxNumComparisons) ;
++ iter) {
vl_index di = tree->dataIndex [iter].index ;
/* multiple KDTrees share the database points and we must avoid
* adding the same point twice */
if (self->searchIdBook[di] == self->searchId) continue ;
self->searchIdBook[di] = self->searchId ;
/* compare the query to this point */
switch (self->dataType) {
case VL_TYPE_FLOAT:
dist = ((VlFloatVectorComparisonFunction)self->distanceFunction)
(self->dimension,
((float const *)query),
((float const*)self->data) + di * self->dimension) ;
break ;
case VL_TYPE_DOUBLE:
dist = ((VlDoubleVectorComparisonFunction)self->distanceFunction)
(self->dimension,
((double const *)query),
((double const*)self->data) + di * self->dimension) ;
break ;
default:
abort() ;
}
self->searchNumComparisons += 1 ;
/* see if it should be added to the result set */
if (*numAddedNeighbors < numNeighbors) {
VlKDForestNeighbor * newNeighbor = neighbors + *numAddedNeighbors ;
newNeighbor->index = di ;
newNeighbor->distance = dist ;
vl_kdforest_neighbor_heap_push (neighbors, numAddedNeighbors) ;
} else {
VlKDForestNeighbor * largestNeighbor = neighbors + 0 ;
if (largestNeighbor->distance > dist) {
largestNeighbor->index = di ;
largestNeighbor->distance = dist ;
vl_kdforest_neighbor_heap_update (neighbors, *numAddedNeighbors, 0) ;
}
}
} /* next data point */
return nodeIndex ;
}
#if 0
assert (x1 <= x2 && x2 <= x3) ;
assert (node->lowerChild >= 0) ;
assert (node->upperChild >= 0) ;
#endif
/*
* x1 x2 x3
* x (---|---]
* (--x|---]
* (---|x--]
* (---|---] x
*/
delta = x - x2 ;
saveDist = dist + delta*delta ;
if (x <= x2) {
nextChild = node->lowerChild ;
saveChild = node->upperChild ;
if (x <= x1) {
delta = x - x1 ;
saveDist -= delta*delta ;
}
} else {
nextChild = node->upperChild ;
saveChild = node->lowerChild ;
if (x > x3) {
delta = x - x3 ;
saveDist -= delta*delta ;
}
}
if (*numAddedNeighbors < numNeighbors || neighbors[0].distance > saveDist) {
searchState = self->searchHeapArray + self->searchHeapNumNodes ;
searchState->tree = tree ;
searchState->nodeIndex = saveChild ;
searchState->distanceLowerBound = saveDist ;
vl_kdforest_search_heap_push (self->searchHeapArray,
&self->searchHeapNumNodes) ;
}
return vl_kdforest_query_recursively (self,
tree,
nextChild,
neighbors,
numNeighbors,
numAddedNeighbors,
dist,
query) ;
}
VL_EXPORT vl_size
vl_kdforest_query (VlKDForest * self,
VlKDForestNeighbor * neighbors,
vl_size numNeighbors,
void const * query)
{
vl_uindex i, ti ;
vl_bool exactSearch = (self->searchMaxNumComparisons == 0) ;
VlKDForestSearchState * searchState ;
vl_size numAddedNeighbors = 0 ;
assert (neighbors) ;
assert (numNeighbors > 0) ;
assert (query) ;
/* this number is used to differentiate a query from the next */
self -> searchId += 1 ;
self -> searchNumRecursions = 0 ;
if (! self -> searchHeapArray) {
/* count number of tree nodes */
/* add support structures */
vl_size maxNumNodes = 0 ;
for (ti = 0 ; ti < self->numTrees ; ++ti) {
maxNumNodes += self->trees[ti]->numUsedNodes ;
}
self -> searchHeapArray = vl_malloc (sizeof(VlKDForestSearchState) * maxNumNodes) ;
self -> searchIdBook = vl_calloc (sizeof(vl_uindex), self->numData) ;
for (ti = 0 ; ti < self->numTrees ; ++ti) {
double * searchBounds = vl_malloc(sizeof(double) * 2 * self->dimension) ;
double * iter = searchBounds ;
double * end = iter + 2 * self->dimension ;
while (iter < end) {
*iter++ = - VL_INFINITY_F ;
*iter++ = + VL_INFINITY_F ;
}
vl_kdtree_calc_bounds_recursively (self->trees[ti], 0, searchBounds) ;
vl_free (searchBounds) ;
}
}
self->searchNumComparisons = 0 ;
self->searchNumSimplifications = 0 ;
/* put the root node into the search heap */
self->searchHeapNumNodes = 0 ;
for (ti = 0 ; ti < self->numTrees ; ++ ti) {
searchState = self->searchHeapArray + self->searchHeapNumNodes ;
searchState -> tree = self->trees[ti] ;
searchState -> nodeIndex = 0 ;
searchState -> distanceLowerBound = 0 ;
vl_kdforest_search_heap_push (self->searchHeapArray, &self->searchHeapNumNodes) ;
}
/* branch and bound */
while (exactSearch || self->searchNumComparisons < self->searchMaxNumComparisons)
{
/* pop the next optimal search node */
VlKDForestSearchState * searchState ;
/* break if search space completed */
if (self->searchHeapNumNodes == 0) {
break ;
}
searchState = self->searchHeapArray +
vl_kdforest_search_heap_pop (self->searchHeapArray, &self->searchHeapNumNodes) ;
/* break if no better solution may exist */
if (numAddedNeighbors == numNeighbors &&
neighbors[0].distance < searchState->distanceLowerBound) {
self->searchNumSimplifications ++ ;
break ;
}
vl_kdforest_query_recursively (self,
searchState->tree,
searchState->nodeIndex,
neighbors,
numNeighbors,
&numAddedNeighbors,
searchState->distanceLowerBound,
query) ;
}
/* sort neighbors by increasing distance */
for (i = numAddedNeighbors ; i < numNeighbors ; ++ i) {
neighbors[i].index = -1 ;
neighbors[i].distance = VL_NAN_F ;
}
while (numAddedNeighbors) {
vl_kdforest_neighbor_heap_pop (neighbors, &numAddedNeighbors) ;
}
return self->searchNumComparisons ;
}
vl_size
vl_kdforestsearcher_query (VlKDForestSearcher * self,
VlKDForestNeighbor * neighbors,
vl_size numNeighbors,
void const * query)
{
vl_uindex i, ti ;
vl_bool exactSearch = self->forest->searchMaxNumComparisons == 0 ;
VlKDForestSearchState * searchState ;
vl_size numAddedNeighbors = 0 ;
assert (neighbors) ;
assert (numNeighbors > 0) ;
assert (query) ;
/* this number is used to differentiate a query from the next */
self -> searchId += 1 ;
self -> searchNumRecursions = 0 ;
self->searchNumComparisons = 0 ;
self->searchNumSimplifications = 0 ;
/* put the root node into the search heap */
self->searchHeapNumNodes = 0 ;
for (ti = 0 ; ti < self->forest->numTrees ; ++ ti) {
searchState = self->searchHeapArray + self->searchHeapNumNodes ;
searchState -> tree = self->forest->trees[ti] ;
searchState -> nodeIndex = 0 ;
searchState -> distanceLowerBound = 0 ;
vl_kdforest_search_heap_push (self->searchHeapArray, &self->searchHeapNumNodes) ;
}
/* branch and bound */
while (exactSearch || self->searchNumComparisons < self->forest->searchMaxNumComparisons)
{
/* pop the next optimal search node */
VlKDForestSearchState * searchState ;
/* break if search space completed */
if (self->searchHeapNumNodes == 0) {
break ;
}
searchState = self->searchHeapArray +
vl_kdforest_search_heap_pop (self->searchHeapArray, &self->searchHeapNumNodes) ;
/* break if no better solution may exist */
if (numAddedNeighbors == numNeighbors &&
neighbors[0].distance < searchState->distanceLowerBound) {
self->searchNumSimplifications ++ ;
break ;
}
vl_kdforest_query_recursively (self,
searchState->tree,
searchState->nodeIndex,
neighbors,
numNeighbors,
&numAddedNeighbors,
searchState->distanceLowerBound,
query) ;
}
/* sort neighbors by increasing distance */
for (i = numAddedNeighbors ; i < numNeighbors ; ++ i) {
neighbors[i].index = -1 ;
neighbors[i].distance = VL_NAN_F ;
}
while (numAddedNeighbors) {
vl_kdforest_neighbor_heap_pop (neighbors, &numAddedNeighbors) ;
}
return self->searchNumComparisons ;
}
