Fawkes API  Fawkes Development Version
pf_kdtree.h
00001 
00002 /***************************************************************************
00003  *  pf_kdtree.h: KD tree functions
00004  *
00005  *  Created: Thu May 24 18:39:48 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /*  From:
00025  *  Player - One Hell of a Robot Server (LGPL)
00026  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00027  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00028  */
00029 /**************************************************************************
00030  * Desc: KD tree functions
00031  * Author: Andrew Howard
00032  * Date: 18 Dec 2002
00033  *************************************************************************/
00034 
00035 #ifndef PF_KDTREE_H
00036 #define PF_KDTREE_H
00037 
00038 #ifdef INCLUDE_RTKGUI
00039 #include "rtk.h"
00040 #endif
00041 
00042 /// @cond EXTERNAL
00043 
00044 // Info for a node in the tree
00045 typedef struct pf_kdtree_node
00046 {
00047   // Depth in the tree
00048   int leaf, depth;
00049 
00050   // Pivot dimension and value
00051   int pivot_dim;
00052   double pivot_value;
00053 
00054   // The key for this node
00055   int key[3];
00056 
00057   // The value for this node
00058   double value;
00059 
00060   // The cluster label (leaf nodes)
00061   int cluster;
00062 
00063   // Child nodes
00064   struct pf_kdtree_node *children[2];
00065 
00066 } pf_kdtree_node_t;
00067 
00068 
00069 // A kd tree
00070 typedef struct
00071 {
00072   // Cell size
00073   double size[3];
00074 
00075   // The root node of the tree
00076   pf_kdtree_node_t *root;
00077 
00078   // The number of nodes in the tree
00079   int node_count, node_max_count;
00080   pf_kdtree_node_t *nodes;
00081 
00082   // The number of leaf nodes in the tree
00083   int leaf_count;
00084 
00085 } pf_kdtree_t;
00086 
00087 
00088 // Create a tree
00089 extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
00090 
00091 // Destroy a tree
00092 extern void pf_kdtree_free(pf_kdtree_t *self);
00093 
00094 // Clear all entries from the tree
00095 extern void pf_kdtree_clear(pf_kdtree_t *self);
00096 
00097 // Insert a pose into the tree
00098 extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
00099 
00100 // Cluster the leaves in the tree
00101 extern void pf_kdtree_cluster(pf_kdtree_t *self);
00102 
00103 // Determine the probability estimate for the given pose
00104 extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
00105 
00106 // Determine the cluster label for the given pose
00107 extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
00108 
00109 
00110 #ifdef INCLUDE_RTKGUI
00111 
00112 // Draw the tree
00113 extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
00114 
00115 #endif
00116 
00117 /// @endcond
00118 
00119 #endif