pf_kdtree.h
Go to the documentation of this file.00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00004 * gerkey@usc.edu kaspers@robotics.usc.edu 00005 * 00006 * This library is free software; you can redistribute it and/or 00007 * modify it under the terms of the GNU Lesser General Public 00008 * License as published by the Free Software Foundation; either 00009 * version 2.1 of the License, or (at your option) any later version. 00010 * 00011 * This library is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00014 * Lesser General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU Lesser General Public 00017 * License along with this library; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 * 00020 */ 00021 /************************************************************************** 00022 * Desc: KD tree functions 00023 * Author: Andrew Howard 00024 * Date: 18 Dec 2002 00025 * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $ 00026 *************************************************************************/ 00027 00028 #ifndef PF_KDTREE_H 00029 #define PF_KDTREE_H 00030 00031 #ifdef INCLUDE_RTKGUI 00032 #include "rtk.h" 00033 #endif 00034 00035 00036 // Info for a node in the tree 00037 typedef struct pf_kdtree_node 00038 { 00039 // Depth in the tree 00040 int leaf, depth; 00041 00042 // Pivot dimension and value 00043 int pivot_dim; 00044 double pivot_value; 00045 00046 // The key for this node 00047 int key[3]; 00048 00049 // The value for this node 00050 double value; 00051 00052 // The cluster label (leaf nodes) 00053 int cluster; 00054 00055 // Child nodes 00056 struct pf_kdtree_node *children[2]; 00057 00058 } pf_kdtree_node_t; 00059 00060 00061 // A kd tree 00062 typedef struct 00063 { 00064 // Cell size 00065 double size[3]; 00066 00067 // The root node of the tree 00068 pf_kdtree_node_t *root; 00069 00070 // The number of nodes in the tree 00071 int node_count, node_max_count; 00072 pf_kdtree_node_t *nodes; 00073 00074 // The number of leaf nodes in the tree 00075 int leaf_count; 00076 00077 } pf_kdtree_t; 00078 00079 00080 // Create a tree 00081 extern pf_kdtree_t *pf_kdtree_alloc(int max_size); 00082 00083 // Destroy a tree 00084 extern void pf_kdtree_free(pf_kdtree_t *self); 00085 00086 // Clear all entries from the tree 00087 extern void pf_kdtree_clear(pf_kdtree_t *self); 00088 00089 // Insert a pose into the tree 00090 extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value); 00091 00092 // Cluster the leaves in the tree 00093 extern void pf_kdtree_cluster(pf_kdtree_t *self); 00094 00095 // Determine the probability estimate for the given pose 00096 extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose); 00097 00098 // Determine the cluster label for the given pose 00099 extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose); 00100 00101 00102 #ifdef INCLUDE_RTKGUI 00103 00104 // Draw the tree 00105 extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig); 00106 00107 #endif 00108 00109 #endif