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


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:02