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

Last updated 25 May 2011 21:17:00