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 #ifdef INCLUDE_RTKGUI
39 #include "rtk.h"
40 #endif
41 
42 /// @cond EXTERNAL
43 
44 // Info for a node in the tree
45 typedef struct pf_kdtree_node
46 {
47  // Depth in the tree
48  int leaf, depth;
49 
50  // Pivot dimension and value
51  int pivot_dim;
52  double pivot_value;
53 
54  // The key for this node
55  int key[3];
56 
57  // The value for this node
58  double value;
59 
60  // The cluster label (leaf nodes)
61  int cluster;
62 
63  // Child nodes
64  struct pf_kdtree_node *children[2];
65 
66 } pf_kdtree_node_t;
67 
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 
88 // Create a tree
89 extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
90 
91 // Destroy a tree
92 extern void pf_kdtree_free(pf_kdtree_t *self);
93 
94 // Clear all entries from the tree
95 extern void pf_kdtree_clear(pf_kdtree_t *self);
96 
97 // Insert a pose into the tree
98 extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
99 
100 // Cluster the leaves in the tree
101 extern void pf_kdtree_cluster(pf_kdtree_t *self);
102 
103 // Determine the probability estimate for the given pose
104 extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
105 
106 // Determine the cluster label for the given pose
107 extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
108 
109 
110 #ifdef INCLUDE_RTKGUI
111 
112 // Draw the tree
113 extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
114 
115 #endif
116 
117 /// @endcond
118 
119 #endif