Tesseract  3.02
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
kdtree.cpp
Go to the documentation of this file.
1 /******************************************************************************
2  ** Filename: kdtree.cpp
3  ** Purpose: Routines for managing K-D search trees
4  ** Author: Dan Johnson
5  ** History: 3/10/89, DSJ, Created.
6  ** 5/23/89, DSJ, Added circular feature capability.
7  ** 7/13/89, DSJ, Made tree nodes invisible to outside.
8  **
9  ** (c) Copyright Hewlett-Packard Company, 1988.
10  ** Licensed under the Apache License, Version 2.0 (the "License");
11  ** you may not use this file except in compliance with the License.
12  ** You may obtain a copy of the License at
13  ** http://www.apache.org/licenses/LICENSE-2.0
14  ** Unless required by applicable law or agreed to in writing, software
15  ** distributed under the License is distributed on an "AS IS" BASIS,
16  ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  ** See the License for the specific language governing permissions and
18  ** limitations under the License.
19  ******************************************************************************/
20 
21 /*-----------------------------------------------------------------------------
22  Include Files and Type Defines
23 -----------------------------------------------------------------------------*/
24 #include "kdtree.h"
25 #include "const.h"
26 #include "emalloc.h"
27 #include "freelist.h"
28 #include <stdio.h>
29 #include <math.h>
30 
31 #define Magnitude(X) ((X) < 0 ? -(X) : (X))
32 #define NodeFound(N,K,D) (( (N)->Key == (K) ) && ( (N)->Data == (D) ))
33 
34 /*-----------------------------------------------------------------------------
35  Global Data Definitions and Declarations
36 -----------------------------------------------------------------------------*/
37 #define MINSEARCH -MAX_FLOAT32
38 #define MAXSEARCH MAX_FLOAT32
39 
40 // Helper function to find the next essential dimension in a cycle.
41 static int NextLevel(KDTREE *tree, int level) {
42  do {
43  ++level;
44  if (level >= tree->KeySize)
45  level = 0;
46  } while (tree->KeyDesc[level].NonEssential);
47  return level;
48 }
49 
50 //-----------------------------------------------------------------------------
51 // Store the k smallest-keyed key-value pairs.
52 template<typename Key, typename Value>
53 class MinK {
54  public:
55  MinK(Key max_key, int k);
56  ~MinK();
57 
58  struct Element {
59  Element() {}
60  Element(const Key& k, const Value& v) : key(k), value(v) {}
61 
62  Key key;
63  Value value;
64  };
65 
66  bool insert(Key k, Value v);
67  const Key& max_insertable_key();
68 
69  int elements_count() { return elements_count_; }
70  const Element* elements() { return elements_; }
71 
72  private:
73  const Key max_key_; // the maximum possible Key
74  Element* elements_; // unsorted array of elements
75  int elements_count_; // the number of results collected so far
76  int k_; // the number of results we want from the search
77  int max_index_; // the index of the result with the largest key
78 };
79 
80 template<typename Key, typename Value>
81 MinK<Key, Value>::MinK(Key max_key, int k) :
82  max_key_(max_key), elements_count_(0), k_(k < 1 ? 1 : k), max_index_(0) {
83  elements_ = new Element[k_];
84 }
85 
86 template<typename Key, typename Value>
88  delete []elements_;
89 }
90 
91 template<typename Key, typename Value>
93  if (elements_count_ < k_)
94  return max_key_;
95  return elements_[max_index_].key;
96 }
97 
98 template<typename Key, typename Value>
99 bool MinK<Key, Value>::insert(Key key, Value value) {
100  if (elements_count_ < k_) {
101  elements_[elements_count_++] = Element(key, value);
102  if (key > elements_[max_index_].key)
103  max_index_ = elements_count_ - 1;
104  return true;
105  } else if (key < elements_[max_index_].key) {
106  // evict the largest element.
107  elements_[max_index_] = Element(key, value);
108  // recompute max_index_
109  for (int i = 0; i < elements_count_; i++) {
110  if (elements_[i].key > elements_[max_index_].key)
111  max_index_ = i;
112  }
113  return true;
114  }
115  return false;
116 }
117 
118 
119 //-----------------------------------------------------------------------------
120 // Helper class for searching for the k closest points to query_point in tree.
122  public:
123  KDTreeSearch(KDTREE* tree, FLOAT32 *query_point, int k_closest);
124  ~KDTreeSearch();
125 
126  // Return the k nearest points' data.
127  void Search(int *result_count, FLOAT32 *distances, void **results);
128 
129  private:
130  void SearchRec(int Level, KDNODE *SubTree);
131  bool BoxIntersectsSearch(FLOAT32 *lower, FLOAT32 *upper);
132 
133  KDTREE *tree_;
134  FLOAT32 *query_point_;
135  MinK<FLOAT32, void *>* results_;
136  FLOAT32 *sb_min_; // search box minimum
137  FLOAT32 *sb_max_; // search box maximum
138 };
139 
140 KDTreeSearch::KDTreeSearch(KDTREE* tree, FLOAT32 *query_point, int k_closest) :
141  tree_(tree),
142  query_point_(query_point) {
143  results_ = new MinK<FLOAT32, void *>(MAXSEARCH, k_closest);
144  sb_min_ = new FLOAT32[tree->KeySize];
145  sb_max_ = new FLOAT32[tree->KeySize];
146 }
147 
149  delete results_;
150  delete[] sb_min_;
151  delete[] sb_max_;
152 }
153 
154 // Locate the k_closest points to query_point_, and return their distances and
155 // data into the given buffers.
156 void KDTreeSearch::Search(int *result_count,
157  FLOAT32 *distances,
158  void **results) {
159  if (tree_->Root.Left == NULL) {
160  *result_count = 0;
161  } else {
162  for (int i = 0; i < tree_->KeySize; i++) {
163  sb_min_[i] = tree_->KeyDesc[i].Min;
164  sb_max_[i] = tree_->KeyDesc[i].Max;
165  }
166  SearchRec(0, tree_->Root.Left);
167  int count = results_->elements_count();
168  *result_count = count;
169  for (int j = 0; j < count; j++) {
170  distances[j] = (FLOAT32) sqrt((FLOAT64)results_->elements()[j].key);
171  results[j] = results_->elements()[j].value;
172  }
173  }
174 }
175 
176 /*-----------------------------------------------------------------------------
177  Public Code
178 -----------------------------------------------------------------------------*/
179 /*---------------------------------------------------------------------------*/
184 KDTREE *MakeKDTree(inT16 KeySize, const PARAM_DESC KeyDesc[]) {
185  KDTREE *KDTree = (KDTREE *) Emalloc(
186  sizeof(KDTREE) + (KeySize - 1) * sizeof(PARAM_DESC));
187  for (int i = 0; i < KeySize; i++) {
188  KDTree->KeyDesc[i].NonEssential = KeyDesc[i].NonEssential;
189  KDTree->KeyDesc[i].Circular = KeyDesc[i].Circular;
190  if (KeyDesc[i].Circular) {
191  KDTree->KeyDesc[i].Min = KeyDesc[i].Min;
192  KDTree->KeyDesc[i].Max = KeyDesc[i].Max;
193  KDTree->KeyDesc[i].Range = KeyDesc[i].Max - KeyDesc[i].Min;
194  KDTree->KeyDesc[i].HalfRange = KDTree->KeyDesc[i].Range / 2;
195  KDTree->KeyDesc[i].MidRange = (KeyDesc[i].Max + KeyDesc[i].Min) / 2;
196  } else {
197  KDTree->KeyDesc[i].Min = MINSEARCH;
198  KDTree->KeyDesc[i].Max = MAXSEARCH;
199  }
200  }
201  KDTree->KeySize = KeySize;
202  KDTree->Root.Left = NULL;
203  KDTree->Root.Right = NULL;
204  return KDTree;
205 }
206 
207 
208 /*---------------------------------------------------------------------------*/
209 void KDStore(KDTREE *Tree, FLOAT32 *Key, void *Data) {
222  int Level;
223  KDNODE *Node;
224  KDNODE **PtrToNode;
225 
226  PtrToNode = &(Tree->Root.Left);
227  Node = *PtrToNode;
228  Level = NextLevel(Tree, -1);
229  while (Node != NULL) {
230  if (Key[Level] < Node->BranchPoint) {
231  PtrToNode = &(Node->Left);
232  if (Key[Level] > Node->LeftBranch)
233  Node->LeftBranch = Key[Level];
234  }
235  else {
236  PtrToNode = &(Node->Right);
237  if (Key[Level] < Node->RightBranch)
238  Node->RightBranch = Key[Level];
239  }
240  Level = NextLevel(Tree, Level);
241  Node = *PtrToNode;
242  }
243 
244  *PtrToNode = MakeKDNode(Tree, Key, (void *) Data, Level);
245 } /* KDStore */
246 
247 
248 /*---------------------------------------------------------------------------*/
268 void
269 KDDelete (KDTREE * Tree, FLOAT32 Key[], void *Data) {
270  int Level;
271  KDNODE *Current;
272  KDNODE *Father;
273 
274  /* initialize search at root of tree */
275  Father = &(Tree->Root);
276  Current = Father->Left;
277  Level = NextLevel(Tree, -1);
278 
279  /* search tree for node to be deleted */
280  while ((Current != NULL) && (!NodeFound (Current, Key, Data))) {
281  Father = Current;
282  if (Key[Level] < Current->BranchPoint)
283  Current = Current->Left;
284  else
285  Current = Current->Right;
286 
287  Level = NextLevel(Tree, Level);
288  }
289 
290  if (Current != NULL) { /* if node to be deleted was found */
291  if (Current == Father->Left) {
292  Father->Left = NULL;
293  Father->LeftBranch = Tree->KeyDesc[Level].Min;
294  } else {
295  Father->Right = NULL;
296  Father->RightBranch = Tree->KeyDesc[Level].Max;
297  }
298 
299  InsertNodes(Tree, Current->Left);
300  InsertNodes(Tree, Current->Right);
301  FreeSubTree(Current);
302  }
303 } /* KDDelete */
304 
305 
306 /*---------------------------------------------------------------------------*/
308  KDTREE *Tree, FLOAT32 Query[], int QuerySize, FLOAT32 MaxDistance,
309  int *NumberOfResults, void **NBuffer, FLOAT32 DBuffer[]) {
310 /*
311  ** Parameters:
312  ** Tree ptr to K-D tree to be searched
313  ** Query ptr to query key (point in D-space)
314  ** QuerySize number of nearest neighbors to be found
315  ** MaxDistance all neighbors must be within this distance
316  ** NBuffer ptr to QuerySize buffer to hold nearest neighbors
317  ** DBuffer ptr to QuerySize buffer to hold distances
318  ** from nearest neighbor to query point
319  ** Operation:
320  ** This routine searches the K-D tree specified by Tree and
321  ** finds the QuerySize nearest neighbors of Query. All neighbors
322  ** must be within MaxDistance of Query. The data contents of
323  ** the nearest neighbors
324  ** are placed in NBuffer and their distances from Query are
325  ** placed in DBuffer.
326  ** Return: Number of nearest neighbors actually found
327  ** Exceptions: none
328  ** History:
329  ** 3/10/89, DSJ, Created.
330  ** 7/13/89, DSJ, Return contents of node instead of node itself.
331  */
332  KDTreeSearch search(Tree, Query, QuerySize);
333  search.Search(NumberOfResults, DBuffer, NBuffer);
334 }
335 
336 
337 /*---------------------------------------------------------------------------*/
338 // Walk a given Tree with action.
339 void KDWalk(KDTREE *Tree, void_proc action, void *context) {
340  if (Tree->Root.Left != NULL)
341  Walk(Tree, action, context, Tree->Root.Left, NextLevel(Tree, -1));
342 }
343 
344 
345 /*---------------------------------------------------------------------------*/
346 void FreeKDTree(KDTREE *Tree) {
347 /*
348  ** Parameters:
349  ** Tree tree data structure to be released
350  ** Operation:
351  ** This routine frees all memory which is allocated to the
352  ** specified KD-tree. This includes the data structure for
353  ** the kd-tree itself plus the data structures for each node
354  ** in the tree. It does not include the Key and Data items
355  ** which are pointed to by the nodes. This memory is left
356  ** untouched.
357  ** Return: none
358  ** Exceptions: none
359  ** History:
360  ** 5/26/89, DSJ, Created.
361  */
362  FreeSubTree(Tree->Root.Left);
363  memfree(Tree);
364 } /* FreeKDTree */
365 
366 
367 /*-----------------------------------------------------------------------------
368  Private Code
369 -----------------------------------------------------------------------------*/
370 /*---------------------------------------------------------------------------*/
371 KDNODE *MakeKDNode(KDTREE *tree, FLOAT32 Key[], void *Data, int Index) {
372 /*
373  ** Parameters:
374  ** tree The tree to create the node for
375  ** Key Access key for new node in KD tree
376  ** Data ptr to data to be stored in new node
377  ** Index index of Key to branch on
378  ** Operation:
379  ** This routine allocates memory for a new K-D tree node
380  ** and places the specified Key and Data into it. The
381  ** left and right subtree pointers for the node are
382  ** initialized to empty subtrees.
383  ** Return:
384  ** pointer to new K-D tree node
385  ** Exceptions:
386  ** None
387  ** History:
388  ** 3/11/89, DSJ, Created.
389  */
390  KDNODE *NewNode;
391 
392  NewNode = (KDNODE *) Emalloc (sizeof (KDNODE));
393 
394  NewNode->Key = Key;
395  NewNode->Data = Data;
396  NewNode->BranchPoint = Key[Index];
397  NewNode->LeftBranch = tree->KeyDesc[Index].Min;
398  NewNode->RightBranch = tree->KeyDesc[Index].Max;
399  NewNode->Left = NULL;
400  NewNode->Right = NULL;
401 
402  return NewNode;
403 } /* MakeKDNode */
404 
405 
406 /*---------------------------------------------------------------------------*/
407 void FreeKDNode(KDNODE *Node) {
408  memfree ((char *)Node);
409 }
410 
411 
412 /*---------------------------------------------------------------------------*/
413 // Recursively accumulate the k_closest points to query_point_ into results_.
414 // Parameters:
415 // Level level in tree of sub-tree to be searched
416 // SubTree sub-tree to be searched
417 void KDTreeSearch::SearchRec(int level, KDNODE *sub_tree) {
418  if (level >= tree_->KeySize)
419  level = 0;
420 
421  if (!BoxIntersectsSearch(sb_min_, sb_max_))
422  return;
423 
424  results_->insert(DistanceSquared(tree_->KeySize, tree_->KeyDesc,
425  query_point_, sub_tree->Key),
426  sub_tree->Data);
427 
428  if (query_point_[level] < sub_tree->BranchPoint) {
429  if (sub_tree->Left != NULL) {
430  FLOAT32 tmp = sb_max_[level];
431  sb_max_[level] = sub_tree->LeftBranch;
432  SearchRec(NextLevel(tree_, level), sub_tree->Left);
433  sb_max_[level] = tmp;
434  }
435  if (sub_tree->Right != NULL) {
436  FLOAT32 tmp = sb_min_[level];
437  sb_min_[level] = sub_tree->RightBranch;
438  SearchRec(NextLevel(tree_, level), sub_tree->Right);
439  sb_min_[level] = tmp;
440  }
441  } else {
442  if (sub_tree->Right != NULL) {
443  FLOAT32 tmp = sb_min_[level];
444  sb_min_[level] = sub_tree->RightBranch;
445  SearchRec(NextLevel(tree_, level), sub_tree->Right);
446  sb_min_[level] = tmp;
447  }
448  if (sub_tree->Left != NULL) {
449  FLOAT32 tmp = sb_max_[level];
450  sb_max_[level] = sub_tree->LeftBranch;
451  SearchRec(NextLevel(tree_, level), sub_tree->Left);
452  sb_max_[level] = tmp;
453  }
454  }
455 }
456 
457 
458 /*---------------------------------------------------------------------------*/
459 // Returns the Euclidean distance squared between p1 and p2 for all essential
460 // dimensions.
461 // Parameters:
462 // k keys are in k-space
463 // dim dimension descriptions (essential, circular, etc)
464 // p1,p2 two different points in K-D space
466  FLOAT32 total_distance = 0;
467 
468  for (; k > 0; k--, p1++, p2++, dim++) {
469  if (dim->NonEssential)
470  continue;
471 
472  FLOAT32 dimension_distance = *p1 - *p2;
473 
474  /* if this dimension is circular - check wraparound distance */
475  if (dim->Circular) {
476  dimension_distance = Magnitude(dimension_distance);
477  FLOAT32 wrap_distance = dim->Max - dim->Min - dimension_distance;
478  dimension_distance = MIN(dimension_distance, wrap_distance);
479  }
480 
481  total_distance += dimension_distance * dimension_distance;
482  }
483  return total_distance;
484 }
485 
487  return sqrt(DistanceSquared(k, dim, p1, p2));
488 }
489 
490 /*---------------------------------------------------------------------------*/
491 // Return whether the query region (the smallest known circle about
492 // query_point_ containing results->k_ points) intersects the box specified
493 // between lower and upper. For circular dimensions, we also check the point
494 // one wrap distance away from the query.
495 bool KDTreeSearch::BoxIntersectsSearch(FLOAT32 *lower, FLOAT32 *upper) {
496  FLOAT32 *query = query_point_;
497  FLOAT64 total_distance = 0.0;
498  FLOAT64 radius_squared =
499  results_->max_insertable_key() * results_->max_insertable_key();
500  PARAM_DESC *dim = tree_->KeyDesc;
501 
502  for (int i = tree_->KeySize; i > 0; i--, dim++, query++, lower++, upper++) {
503  if (dim->NonEssential)
504  continue;
505 
506  FLOAT32 dimension_distance;
507  if (*query < *lower)
508  dimension_distance = *lower - *query;
509  else if (*query > *upper)
510  dimension_distance = *query - *upper;
511  else
512  dimension_distance = 0;
513 
514  /* if this dimension is circular - check wraparound distance */
515  if (dim->Circular) {
516  FLOAT32 wrap_distance = MAX_FLOAT32;
517  if (*query < *lower)
518  wrap_distance = *query + dim->Max - dim->Min - *upper;
519  else if (*query > *upper)
520  wrap_distance = *lower - (*query - (dim->Max - dim->Min));
521  dimension_distance = MIN(dimension_distance, wrap_distance);
522  }
523 
524  total_distance += dimension_distance * dimension_distance;
525  if (total_distance >= radius_squared)
526  return FALSE;
527  }
528  return TRUE;
529 }
530 
531 
532 /*---------------------------------------------------------------------------*/
533 // Walk a tree, calling action once on each node.
534 //
535 // Parameters:
536 // tree root of the tree being walked.
537 // action action to be performed at every node
538 // context action's context
539 // sub_tree ptr to root of subtree to be walked
540 // level current level in the tree for this node
541 // Operation:
542 // This routine walks thru the specified sub_tree and invokes action
543 // action at each node as follows:
544 // action(context, data, level)
545 // data the data contents of the node being visited,
546 // level is the level of the node in the tree with the root being level 0.
547 void Walk(KDTREE *tree, void_proc action, void *context,
548  KDNODE *sub_tree, inT32 level) {
549  (*action)(context, sub_tree->Data, level);
550  if (sub_tree->Left != NULL)
551  Walk(tree, action, context, sub_tree->Left, NextLevel(tree, level));
552  if (sub_tree->Right != NULL)
553  Walk(tree, action, context, sub_tree->Right, NextLevel(tree, level));
554 }
555 
556 
557 // Given a subtree nodes, insert all of its elements into tree.
558 void InsertNodes(KDTREE *tree, KDNODE *nodes) {
559  if (nodes == NULL)
560  return;
561 
562  KDStore(tree, nodes->Key, nodes->Data);
563  InsertNodes(tree, nodes->Left);
564  InsertNodes(tree, nodes->Right);
565 }
566 
567 // Free all of the nodes of a sub tree.
568 void FreeSubTree(KDNODE *sub_tree) {
569  if (sub_tree != NULL) {
570  FreeSubTree(sub_tree->Left);
571  FreeSubTree(sub_tree->Right);
572  memfree(sub_tree);
573  }
574 } /* FreeSubTree */