Fawkes API Fawkes Development Version
pf_kdtree.h
1
2/***************************************************************************
3 * pf_kdtree.h: KD tree functions
4 *
5 * Created: Thu May 24 18:39:48 2012
6 * Copyright 2000 Brian Gerkey
7 * 2000 Kasper Stoy
8 * 2012 Tim Niemueller [www.niemueller.de]
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24/* From:
25 * Player - One Hell of a Robot Server (LGPL)
26 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27 * gerkey@usc.edu kaspers@robotics.usc.edu
28 */
29/**************************************************************************
30 * Desc: KD tree functions
31 * Author: Andrew Howard
32 * Date: 18 Dec 2002
33 *************************************************************************/
34
35#ifndef PF_KDTREE_H
36#define PF_KDTREE_H
37
38#include "pf_vector.h"
39#ifdef INCLUDE_RTKGUI
40# include "rtk.h"
41#endif
42
43/// @cond EXTERNAL
44
45// Info for a node in the tree
46typedef struct pf_kdtree_node
47{
48 // Depth in the tree
49 int leaf, depth;
50
51 // Pivot dimension and value
52 int pivot_dim;
53 double pivot_value;
54
55 // The key for this node
56 int key[3];
57
58 // The value for this node
59 double value;
60
61 // The cluster label (leaf nodes)
62 int cluster;
63
64 // Child nodes
65 struct pf_kdtree_node *children[2];
66
67} pf_kdtree_node_t;
68
69// A kd tree
70typedef struct
71{
72 // Cell size
73 double size[3];
74
75 // The root node of the tree
76 pf_kdtree_node_t *root;
77
78 // The number of nodes in the tree
79 int node_count, node_max_count;
80 pf_kdtree_node_t *nodes;
81
82 // The number of leaf nodes in the tree
83 int leaf_count;
84
85} pf_kdtree_t;
86
87// Create a tree
88extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
89
90// Destroy a tree
91extern void pf_kdtree_free(pf_kdtree_t *self);
92
93// Clear all entries from the tree
94extern void pf_kdtree_clear(pf_kdtree_t *self);
95
96// Insert a pose into the tree
97extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
98
99// Cluster the leaves in the tree
100extern void pf_kdtree_cluster(pf_kdtree_t *self);
101
102// Determine the probability estimate for the given pose
103extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
104
105// Determine the cluster label for the given pose
106extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
107
108#ifdef INCLUDE_RTKGUI
109
110// Draw the tree
111extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
112
113#endif
114
115/// @endcond
116
117#endif