pf_kdtree.c
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.c 7057 2008-10-02 00:44:06Z gbiggs $
00026  *************************************************************************/
00027 
00028 #include <assert.h>
00029 #include <math.h>
00030 #include <stdlib.h>
00031 #include <string.h>
00032 
00033 
00034 #include "amcl/pf/pf_vector.h"
00035 #include "amcl/pf/pf_kdtree.h"
00036 
00037 
00038 // Compare keys to see if they are equal
00039 static int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]);
00040 
00041 // Insert a node into the tree
00042 static pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent,
00043                                                pf_kdtree_node_t *node, int key[], double value);
00044 
00045 // Recursive node search
00046 static pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]);
00047 
00048 // Recursively label nodes in this cluster
00049 static void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth);
00050 
00051 // Recursive node printing
00052 //static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node);
00053 
00054 
00055 #ifdef INCLUDE_RTKGUI
00056 
00057 // Recursively draw nodes
00058 static void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig);
00059 
00060 #endif
00061 
00062 
00063 
00065 // Create a tree
00066 pf_kdtree_t *pf_kdtree_alloc(int max_size)
00067 {
00068   pf_kdtree_t *self;
00069 
00070   self = calloc(1, sizeof(pf_kdtree_t));
00071 
00072   self->size[0] = 0.50;
00073   self->size[1] = 0.50;
00074   self->size[2] = (10 * M_PI / 180);
00075 
00076   self->root = NULL;
00077 
00078   self->node_count = 0;
00079   self->node_max_count = max_size;
00080   self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t));
00081 
00082   self->leaf_count = 0;
00083 
00084   return self;
00085 }
00086 
00087 
00089 // Destroy a tree
00090 void pf_kdtree_free(pf_kdtree_t *self)
00091 {
00092   free(self->nodes);
00093   free(self);
00094   return;
00095 }
00096 
00097 
00099 // Clear all entries from the tree
00100 void pf_kdtree_clear(pf_kdtree_t *self)
00101 {
00102   self->root = NULL;
00103   self->leaf_count = 0;
00104   self->node_count = 0;
00105 
00106   return;
00107 }
00108 
00109 
00111 // Insert a pose into the tree.
00112 void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
00113 {
00114   int key[3];
00115 
00116   key[0] = floor(pose.v[0] / self->size[0]);
00117   key[1] = floor(pose.v[1] / self->size[1]);
00118   key[2] = floor(pose.v[2] / self->size[2]);
00119 
00120   self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);
00121 
00122   // Test code
00123   /*
00124   printf("find %d %d %d\n", key[0], key[1], key[2]);
00125   assert(pf_kdtree_find_node(self, self->root, key) != NULL);
00126 
00127   pf_kdtree_print_node(self, self->root);
00128 
00129   printf("\n");
00130 
00131   for (i = 0; i < self->node_count; i++)
00132   {
00133     node = self->nodes + i;
00134     if (node->leaf)
00135     {
00136       printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]);
00137       assert(pf_kdtree_find_node(self, self->root, node->key) == node);
00138     }
00139   }
00140   printf("\n\n");
00141   */
00142 
00143   return;
00144 }
00145 
00146 
00148 // Determine the probability estimate for the given pose. TODO: this
00149 // should do a kernel density estimate rather than a simple histogram.
00150 double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose)
00151 {
00152   int key[3];
00153   pf_kdtree_node_t *node;
00154 
00155   key[0] = floor(pose.v[0] / self->size[0]);
00156   key[1] = floor(pose.v[1] / self->size[1]);
00157   key[2] = floor(pose.v[2] / self->size[2]);
00158 
00159   node = pf_kdtree_find_node(self, self->root, key);
00160   if (node == NULL)
00161     return 0.0;
00162   return node->value;
00163 }
00164 
00165 
00167 // Determine the cluster label for the given pose
00168 int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
00169 {
00170   int key[3];
00171   pf_kdtree_node_t *node;
00172 
00173   key[0] = floor(pose.v[0] / self->size[0]);
00174   key[1] = floor(pose.v[1] / self->size[1]);
00175   key[2] = floor(pose.v[2] / self->size[2]);
00176 
00177   node = pf_kdtree_find_node(self, self->root, key);
00178   if (node == NULL)
00179     return -1;
00180   return node->cluster;
00181 }
00182 
00183 
00185 // Compare keys to see if they are equal
00186 int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[])
00187 {
00188   //double a, b;
00189 
00190   if (key_a[0] != key_b[0])
00191     return 0;
00192   if (key_a[1] != key_b[1])
00193     return 0;
00194 
00195   if (key_a[2] != key_b[2])
00196     return 0;
00197 
00198   /* TODO: make this work (pivot selection needs fixing, too)
00199   // Normalize angles
00200   a = key_a[2] * self->size[2];
00201   a = atan2(sin(a), cos(a)) / self->size[2];
00202   b = key_b[2] * self->size[2];
00203   b = atan2(sin(b), cos(b)) / self->size[2];
00204 
00205  if ((int) a != (int) b)
00206     return 0;
00207   */
00208 
00209   return 1;
00210 }
00211 
00212 
00214 // Insert a node into the tree
00215 pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent,
00216                                         pf_kdtree_node_t *node, int key[], double value)
00217 {
00218   int i;
00219   int split, max_split;
00220 
00221   // If the node doesnt exist yet...
00222   if (node == NULL)
00223   {
00224     assert(self->node_count < self->node_max_count);
00225     node = self->nodes + self->node_count++;
00226     memset(node, 0, sizeof(pf_kdtree_node_t));
00227 
00228     node->leaf = 1;
00229 
00230     if (parent == NULL)
00231       node->depth = 0;
00232     else
00233       node->depth = parent->depth + 1;
00234 
00235     for (i = 0; i < 3; i++)
00236       node->key[i] = key[i];
00237 
00238     node->value = value;
00239     self->leaf_count += 1;
00240   }
00241 
00242   // If the node exists, and it is a leaf node...
00243   else if (node->leaf)
00244   {
00245     // If the keys are equal, increment the value
00246     if (pf_kdtree_equal(self, key, node->key))
00247     {
00248       node->value += value;
00249     }
00250 
00251     // The keys are not equal, so split this node
00252     else
00253     {
00254       // Find the dimension with the largest variance and do a mean
00255       // split
00256       max_split = 0;
00257       node->pivot_dim = -1;
00258       for (i = 0; i < 3; i++)
00259       {
00260         split = abs(key[i] - node->key[i]);
00261         if (split > max_split)
00262         {
00263           max_split = split;
00264           node->pivot_dim = i;
00265         }
00266       }
00267       assert(node->pivot_dim >= 0);
00268 
00269       node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0;
00270 
00271       if (key[node->pivot_dim] < node->pivot_value)
00272       {
00273         node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value);
00274         node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
00275       }
00276       else
00277       {
00278         node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
00279         node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value);
00280       }
00281 
00282       node->leaf = 0;
00283       self->leaf_count -= 1;
00284     }
00285   }
00286 
00287   // If the node exists, and it has children...
00288   else
00289   {
00290     assert(node->children[0] != NULL);
00291     assert(node->children[1] != NULL);
00292 
00293     if (key[node->pivot_dim] < node->pivot_value)
00294       pf_kdtree_insert_node(self, node, node->children[0], key, value);
00295     else
00296       pf_kdtree_insert_node(self, node, node->children[1], key, value);
00297   }
00298 
00299   return node;
00300 }
00301 
00302 
00304 // Recursive node search
00305 pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[])
00306 {
00307   if (node->leaf)
00308   {
00309     //printf("find  : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]);
00310 
00311     // If the keys are the same...
00312     if (pf_kdtree_equal(self, key, node->key))
00313       return node;
00314     else
00315       return NULL;
00316   }
00317   else
00318   {
00319     //printf("find  : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value);
00320 
00321     assert(node->children[0] != NULL);
00322     assert(node->children[1] != NULL);
00323 
00324     // If the keys are different...
00325     if (key[node->pivot_dim] < node->pivot_value)
00326       return pf_kdtree_find_node(self, node->children[0], key);
00327     else
00328       return pf_kdtree_find_node(self, node->children[1], key);
00329   }
00330 
00331   return NULL;
00332 }
00333 
00334 
00336 // Recursive node printing
00337 /*
00338 void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node)
00339 {
00340   if (node->leaf)
00341   {
00342     printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]);
00343     printf("%*s", node->depth * 11, "");
00344   }
00345   else
00346   {
00347     printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]);
00348     pf_kdtree_print_node(self, node->children[0]);
00349     pf_kdtree_print_node(self, node->children[1]);
00350   }
00351   return;
00352 }
00353 */
00354 
00355 
00357 // Cluster the leaves in the tree
00358 void pf_kdtree_cluster(pf_kdtree_t *self)
00359 {
00360   int i;
00361   int queue_count, cluster_count;
00362   pf_kdtree_node_t **queue, *node;
00363 
00364   queue_count = 0;
00365   queue = calloc(self->node_count, sizeof(queue[0]));
00366 
00367   // Put all the leaves in a queue
00368   for (i = 0; i < self->node_count; i++)
00369   {
00370     node = self->nodes + i;
00371     if (node->leaf)
00372     {
00373       node->cluster = -1;
00374       assert(queue_count < self->node_count);
00375       queue[queue_count++] = node;
00376 
00377       // TESTING; remove
00378       assert(node == pf_kdtree_find_node(self, self->root, node->key));
00379     }
00380   }
00381 
00382   cluster_count = 0;
00383 
00384   // Do connected components for each node
00385   while (queue_count > 0)
00386   {
00387     node = queue[--queue_count];
00388 
00389     // If this node has already been labelled, skip it
00390     if (node->cluster >= 0)
00391       continue;
00392 
00393     // Assign a label to this cluster
00394     node->cluster = cluster_count++;
00395 
00396     // Recursively label nodes in this cluster
00397     pf_kdtree_cluster_node(self, node, 0);
00398   }
00399 
00400   free(queue);
00401   return;
00402 }
00403 
00404 
00406 // Recursively label nodes in this cluster
00407 void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth)
00408 {
00409   int i;
00410   int nkey[3];
00411   pf_kdtree_node_t *nnode;
00412 
00413   for (i = 0; i < 3 * 3 * 3; i++)
00414   {
00415     nkey[0] = node->key[0] + (i / 9) - 1;
00416     nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
00417     nkey[2] = node->key[2] + ((i % 9) % 3) - 1;
00418 
00419     nnode = pf_kdtree_find_node(self, self->root, nkey);
00420     if (nnode == NULL)
00421       continue;
00422 
00423     assert(nnode->leaf);
00424 
00425     // This node already has a label; skip it.  The label should be
00426     // consistent, however.
00427     if (nnode->cluster >= 0)
00428     {
00429       assert(nnode->cluster == node->cluster);
00430       continue;
00431     }
00432 
00433     // Label this node and recurse
00434     nnode->cluster = node->cluster;
00435 
00436     pf_kdtree_cluster_node(self, nnode, depth + 1);
00437   }
00438   return;
00439 }
00440 
00441 
00442 
00443 #ifdef INCLUDE_RTKGUI
00444 
00446 // Draw the tree
00447 void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig)
00448 {
00449   if (self->root != NULL)
00450     pf_kdtree_draw_node(self, self->root, fig);
00451   return;
00452 }
00453 
00454 
00456 // Recursively draw nodes
00457 void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig)
00458 {
00459   double ox, oy;
00460   char text[64];
00461 
00462   if (node->leaf)
00463   {
00464     ox = (node->key[0] + 0.5) * self->size[0];
00465     oy = (node->key[1] + 0.5) * self->size[1];
00466 
00467     rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0);
00468 
00469     //snprintf(text, sizeof(text), "%0.3f", node->value);
00470     //rtk_fig_text(fig, ox, oy, 0.0, text);
00471 
00472     snprintf(text, sizeof(text), "%d", node->cluster);
00473     rtk_fig_text(fig, ox, oy, 0.0, text);
00474   }
00475   else
00476   {
00477     assert(node->children[0] != NULL);
00478     assert(node->children[1] != NULL);
00479     pf_kdtree_draw_node(self, node->children[0], fig);
00480     pf_kdtree_draw_node(self, node->children[1], fig);
00481   }
00482 
00483   return;
00484 }
00485 
00486 #endif


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