Fawkes API
Fawkes Development Version
|
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