pf_kdtree.h
00001 00002 /************************************************************************** 00003 * Desc: KD tree functions 00004 * Author: Andrew Howard 00005 * Date: 18 Dec 2002 00006 * CVS: $Id: pf_kdtree.h 1397 2003-05-16 02:05:56Z inspectorg $ 00007 *************************************************************************/ 00008 00009 #ifndef PF_KDTREE_H 00010 #define PF_KDTREE_H 00011 00012 00013 #ifdef INCLUDE_RTKGUI 00014 #include "rtk.h" 00015 #endif 00016 00017 00018 // Info for a node in the tree 00019 typedef struct pf_kdtree_node 00020 { 00021 // Depth in the tree 00022 int leaf, depth; 00023 00024 // Pivot dimension and value 00025 int pivot_dim; 00026 double pivot_value; 00027 00028 // The key for this node 00029 int key[3]; 00030 00031 // The value for this node 00032 double value; 00033 00034 // The cluster label (leaf nodes) 00035 int cluster; 00036 00037 // Child nodes 00038 struct pf_kdtree_node *children[2]; 00039 00040 } pf_kdtree_node_t; 00041 00042 00043 // A kd tree 00044 typedef struct 00045 { 00046 // Cell size 00047 double size[3]; 00048 00049 // The root node of the tree 00050 pf_kdtree_node_t *root; 00051 00052 // The number of nodes in the tree 00053 int node_count, node_max_count; 00054 pf_kdtree_node_t *nodes; 00055 00056 // The number of leaf nodes in the tree 00057 int leaf_count; 00058 00059 } pf_kdtree_t; 00060 00061 00062 // Create a tree 00063 extern pf_kdtree_t *pf_kdtree_alloc(int max_size); 00064 00065 // Destroy a tree 00066 extern void pf_kdtree_free(pf_kdtree_t *self); 00067 00068 // Clear all entries from the tree 00069 extern void pf_kdtree_clear(pf_kdtree_t *self); 00070 00071 // Insert a pose into the tree 00072 extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value); 00073 00074 // Cluster the leaves in the tree 00075 extern void pf_kdtree_cluster(pf_kdtree_t *self); 00076 00077 // Determine the probability estimate for the given pose 00078 extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose); 00079 00080 // Determine the cluster label for the given pose 00081 extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose); 00082 00083 00084 #ifdef INCLUDE_RTKGUI 00085 00086 // Draw the tree 00087 extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig); 00088 00089 #endif 00090 00091 #endif