pf_kdtree.c
Go to the documentation of this file.
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
4  * gerkey@usc.edu kaspers@robotics.usc.edu
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  *
20  */
21 /**************************************************************************
22  * Desc: kd-tree functions
23  * Author: Andrew Howard
24  * Date: 18 Dec 2002
25  * CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $
26  *************************************************************************/
27 
28 #include <assert.h>
29 #include <math.h>
30 #include <stdlib.h>
31 #include <string.h>
32 
33 
34 #include "amcl/pf/pf_vector.h"
35 #include "amcl/pf/pf_kdtree.h"
36 
37 
38 // Compare keys to see if they are equal
39 static int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]);
40 
41 // Insert a node into the tree
43  pf_kdtree_node_t *node, int key[], double value);
44 
45 // Recursive node search
46 static pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]);
47 
48 // Recursively label nodes in this cluster
49 static void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth);
50 
51 // Recursive node printing
52 //static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node);
53 
54 
55 #ifdef INCLUDE_RTKGUI
56 
57 // Recursively draw nodes
58 static void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig);
59 
60 #endif
61 
62 
63 
65 // Create a tree
67 {
68  pf_kdtree_t *self;
69 
70  self = calloc(1, sizeof(pf_kdtree_t));
71 
72  self->size[0] = 0.50;
73  self->size[1] = 0.50;
74  self->size[2] = (10 * M_PI / 180);
75 
76  self->root = NULL;
77 
78  self->node_count = 0;
79  self->node_max_count = max_size;
80  self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t));
81 
82  self->leaf_count = 0;
83 
84  return self;
85 }
86 
87 
89 // Destroy a tree
91 {
92  free(self->nodes);
93  free(self);
94  return;
95 }
96 
97 
99 // Clear all entries from the tree
101 {
102  self->root = NULL;
103  self->leaf_count = 0;
104  self->node_count = 0;
105 
106  return;
107 }
108 
109 
111 // Insert a pose into the tree.
112 void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
113 {
114  int key[3];
115 
116  key[0] = floor(pose.v[0] / self->size[0]);
117  key[1] = floor(pose.v[1] / self->size[1]);
118  key[2] = floor(pose.v[2] / self->size[2]);
119 
120  self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);
121 
122  // Test code
123  /*
124  printf("find %d %d %d\n", key[0], key[1], key[2]);
125  assert(pf_kdtree_find_node(self, self->root, key) != NULL);
126 
127  pf_kdtree_print_node(self, self->root);
128 
129  printf("\n");
130 
131  for (i = 0; i < self->node_count; i++)
132  {
133  node = self->nodes + i;
134  if (node->leaf)
135  {
136  printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]);
137  assert(pf_kdtree_find_node(self, self->root, node->key) == node);
138  }
139  }
140  printf("\n\n");
141  */
142 
143  return;
144 }
145 
146 
148 // Determine the probability estimate for the given pose. TODO: this
149 // should do a kernel density estimate rather than a simple histogram.
151 {
152  int key[3];
153  pf_kdtree_node_t *node;
154 
155  key[0] = floor(pose.v[0] / self->size[0]);
156  key[1] = floor(pose.v[1] / self->size[1]);
157  key[2] = floor(pose.v[2] / self->size[2]);
158 
159  node = pf_kdtree_find_node(self, self->root, key);
160  if (node == NULL)
161  return 0.0;
162  return node->value;
163 }
164 
165 
167 // Determine the cluster label for the given pose
169 {
170  int key[3];
171  pf_kdtree_node_t *node;
172 
173  key[0] = floor(pose.v[0] / self->size[0]);
174  key[1] = floor(pose.v[1] / self->size[1]);
175  key[2] = floor(pose.v[2] / self->size[2]);
176 
177  node = pf_kdtree_find_node(self, self->root, key);
178  if (node == NULL)
179  return -1;
180  return node->cluster;
181 }
182 
183 
185 // Compare keys to see if they are equal
186 int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[])
187 {
188  //double a, b;
189 
190  if (key_a[0] != key_b[0])
191  return 0;
192  if (key_a[1] != key_b[1])
193  return 0;
194 
195  if (key_a[2] != key_b[2])
196  return 0;
197 
198  /* TODO: make this work (pivot selection needs fixing, too)
199  // Normalize angles
200  a = key_a[2] * self->size[2];
201  a = atan2(sin(a), cos(a)) / self->size[2];
202  b = key_b[2] * self->size[2];
203  b = atan2(sin(b), cos(b)) / self->size[2];
204 
205  if ((int) a != (int) b)
206  return 0;
207  */
208 
209  return 1;
210 }
211 
212 
214 // Insert a node into the tree
216  pf_kdtree_node_t *node, int key[], double value)
217 {
218  int i;
219  int split, max_split;
220 
221  // If the node doesnt exist yet...
222  if (node == NULL)
223  {
224  assert(self->node_count < self->node_max_count);
225  node = self->nodes + self->node_count++;
226  memset(node, 0, sizeof(pf_kdtree_node_t));
227 
228  node->leaf = 1;
229 
230  if (parent == NULL)
231  node->depth = 0;
232  else
233  node->depth = parent->depth + 1;
234 
235  for (i = 0; i < 3; i++)
236  node->key[i] = key[i];
237 
238  node->value = value;
239  self->leaf_count += 1;
240  }
241 
242  // If the node exists, and it is a leaf node...
243  else if (node->leaf)
244  {
245  // If the keys are equal, increment the value
246  if (pf_kdtree_equal(self, key, node->key))
247  {
248  node->value += value;
249  }
250 
251  // The keys are not equal, so split this node
252  else
253  {
254  // Find the dimension with the largest variance and do a mean
255  // split
256  max_split = 0;
257  node->pivot_dim = -1;
258  for (i = 0; i < 3; i++)
259  {
260  split = abs(key[i] - node->key[i]);
261  if (split > max_split)
262  {
263  max_split = split;
264  node->pivot_dim = i;
265  }
266  }
267  assert(node->pivot_dim >= 0);
268 
269  node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0;
270 
271  if (key[node->pivot_dim] < node->pivot_value)
272  {
273  node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value);
274  node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
275  }
276  else
277  {
278  node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
279  node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value);
280  }
281 
282  node->leaf = 0;
283  self->leaf_count -= 1;
284  }
285  }
286 
287  // If the node exists, and it has children...
288  else
289  {
290  assert(node->children[0] != NULL);
291  assert(node->children[1] != NULL);
292 
293  if (key[node->pivot_dim] < node->pivot_value)
294  pf_kdtree_insert_node(self, node, node->children[0], key, value);
295  else
296  pf_kdtree_insert_node(self, node, node->children[1], key, value);
297  }
298 
299  return node;
300 }
301 
302 
304 // Recursive node search
306 {
307  if (node->leaf)
308  {
309  //printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]);
310 
311  // If the keys are the same...
312  if (pf_kdtree_equal(self, key, node->key))
313  return node;
314  else
315  return NULL;
316  }
317  else
318  {
319  //printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value);
320 
321  assert(node->children[0] != NULL);
322  assert(node->children[1] != NULL);
323 
324  // If the keys are different...
325  if (key[node->pivot_dim] < node->pivot_value)
326  return pf_kdtree_find_node(self, node->children[0], key);
327  else
328  return pf_kdtree_find_node(self, node->children[1], key);
329  }
330 
331  return NULL;
332 }
333 
334 
336 // Recursive node printing
337 /*
338 void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node)
339 {
340  if (node->leaf)
341  {
342  printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]);
343  printf("%*s", node->depth * 11, "");
344  }
345  else
346  {
347  printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]);
348  pf_kdtree_print_node(self, node->children[0]);
349  pf_kdtree_print_node(self, node->children[1]);
350  }
351  return;
352 }
353 */
354 
355 
357 // Cluster the leaves in the tree
359 {
360  int i;
361  int queue_count, cluster_count;
362  pf_kdtree_node_t **queue, *node;
363 
364  queue_count = 0;
365  queue = calloc(self->node_count, sizeof(queue[0]));
366 
367  // Put all the leaves in a queue
368  for (i = 0; i < self->node_count; i++)
369  {
370  node = self->nodes + i;
371  if (node->leaf)
372  {
373  node->cluster = -1;
374  assert(queue_count < self->node_count);
375  queue[queue_count++] = node;
376 
377  // TESTING; remove
378  assert(node == pf_kdtree_find_node(self, self->root, node->key));
379  }
380  }
381 
382  cluster_count = 0;
383 
384  // Do connected components for each node
385  while (queue_count > 0)
386  {
387  node = queue[--queue_count];
388 
389  // If this node has already been labelled, skip it
390  if (node->cluster >= 0)
391  continue;
392 
393  // Assign a label to this cluster
394  node->cluster = cluster_count++;
395 
396  // Recursively label nodes in this cluster
397  pf_kdtree_cluster_node(self, node, 0);
398  }
399 
400  free(queue);
401  return;
402 }
403 
404 
406 // Recursively label nodes in this cluster
408 {
409  int i;
410  int nkey[3];
411  pf_kdtree_node_t *nnode;
412 
413  for (i = 0; i < 3 * 3 * 3; i++)
414  {
415  nkey[0] = node->key[0] + (i / 9) - 1;
416  nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
417  nkey[2] = node->key[2] + ((i % 9) % 3) - 1;
418 
419  nnode = pf_kdtree_find_node(self, self->root, nkey);
420  if (nnode == NULL)
421  continue;
422 
423  assert(nnode->leaf);
424 
425  // This node already has a label; skip it. The label should be
426  // consistent, however.
427  if (nnode->cluster >= 0)
428  {
429  assert(nnode->cluster == node->cluster);
430  continue;
431  }
432 
433  // Label this node and recurse
434  nnode->cluster = node->cluster;
435 
436  pf_kdtree_cluster_node(self, nnode, depth + 1);
437  }
438  return;
439 }
440 
441 
442 
443 #ifdef INCLUDE_RTKGUI
444 
446 // Draw the tree
447 void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig)
448 {
449  if (self->root != NULL)
450  pf_kdtree_draw_node(self, self->root, fig);
451  return;
452 }
453 
454 
456 // Recursively draw nodes
457 void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig)
458 {
459  double ox, oy;
460  char text[64];
461 
462  if (node->leaf)
463  {
464  ox = (node->key[0] + 0.5) * self->size[0];
465  oy = (node->key[1] + 0.5) * self->size[1];
466 
467  rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0);
468 
469  //snprintf(text, sizeof(text), "%0.3f", node->value);
470  //rtk_fig_text(fig, ox, oy, 0.0, text);
471 
472  snprintf(text, sizeof(text), "%d", node->cluster);
473  rtk_fig_text(fig, ox, oy, 0.0, text);
474  }
475  else
476  {
477  assert(node->children[0] != NULL);
478  assert(node->children[1] != NULL);
479  pf_kdtree_draw_node(self, node->children[0], fig);
480  pf_kdtree_draw_node(self, node->children[1], fig);
481  }
482 
483  return;
484 }
485 
486 #endif
static void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth)
Definition: pf_kdtree.c:407
void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
Definition: pf_kdtree.c:112
double v[3]
Definition: pf_vector.h:40
int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
Definition: pf_kdtree.c:168
struct pf_kdtree_node * children[2]
Definition: pf_kdtree.h:56
static int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[])
Definition: pf_kdtree.c:186
pf_kdtree_t * pf_kdtree_alloc(int max_size)
Definition: pf_kdtree.c:66
static pf_kdtree_node_t * pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[])
Definition: pf_kdtree.c:305
int key[3]
Definition: pf_kdtree.h:47
double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose)
Definition: pf_kdtree.c:150
void pf_kdtree_clear(pf_kdtree_t *self)
Definition: pf_kdtree.c:100
static pf_kdtree_node_t * pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent, pf_kdtree_node_t *node, int key[], double value)
Definition: pf_kdtree.c:215
void pf_kdtree_free(pf_kdtree_t *self)
Definition: pf_kdtree.c:90
void pf_kdtree_cluster(pf_kdtree_t *self)
Definition: pf_kdtree.c:358
double pivot_value
Definition: pf_kdtree.h:44
double value
Definition: pf_kdtree.h:50


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:36