Fawkes API  Fawkes Development Version
pf_kdtree.h
1 
2 /***************************************************************************
3  * pf_kdtree.h: KD tree functions
4  *
5  * Created: Thu May 24 18:39:48 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 /**************************************************************************
30  * Desc: KD tree functions
31  * Author: Andrew Howard
32  * Date: 18 Dec 2002
33  *************************************************************************/
34 
35 #ifndef PF_KDTREE_H
36 #define PF_KDTREE_H
37 
38 #include "pf_vector.h"
39 #ifdef INCLUDE_RTKGUI
40 # include "rtk.h"
41 #endif
42 
43 /// @cond EXTERNAL
44 
45 // Info for a node in the tree
46 typedef struct pf_kdtree_node
47 {
48  // Depth in the tree
49  int leaf, depth;
50 
51  // Pivot dimension and value
52  int pivot_dim;
53  double pivot_value;
54 
55  // The key for this node
56  int key[3];
57 
58  // The value for this node
59  double value;
60 
61  // The cluster label (leaf nodes)
62  int cluster;
63 
64  // Child nodes
65  struct pf_kdtree_node *children[2];
66 
67 } pf_kdtree_node_t;
68 
69 // A kd tree
70 typedef struct
71 {
72  // Cell size
73  double size[3];
74 
75  // The root node of the tree
76  pf_kdtree_node_t *root;
77 
78  // The number of nodes in the tree
79  int node_count, node_max_count;
80  pf_kdtree_node_t *nodes;
81 
82  // The number of leaf nodes in the tree
83  int leaf_count;
84 
85 } pf_kdtree_t;
86 
87 // Create a tree
88 extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
89 
90 // Destroy a tree
91 extern void pf_kdtree_free(pf_kdtree_t *self);
92 
93 // Clear all entries from the tree
94 extern void pf_kdtree_clear(pf_kdtree_t *self);
95 
96 // Insert a pose into the tree
97 extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
98 
99 // Cluster the leaves in the tree
100 extern void pf_kdtree_cluster(pf_kdtree_t *self);
101 
102 // Determine the probability estimate for the given pose
103 extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
104 
105 // Determine the cluster label for the given pose
106 extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
107 
108 #ifdef INCLUDE_RTKGUI
109 
110 // Draw the tree
111 extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
112 
113 #endif
114 
115 /// @endcond
116 
117 #endif