amcl 代码研究(6)——pf_kdtree(1)

amcl中采用了粒子滤波方法，其中的数据结构为kd-tree，现分析如下：

typedef struct
{
  double v[3];
} pf_vector_t;      //向量

// The basic matrix
typedef struct
{
  double m[3][3];
} pf_matrix_t;        //矩阵

KD-Tree:

// Info for a node in the tree
typedef struct pf_kdtree_node
{
  // Depth in the tree
  int leaf, depth;

  // Pivot dimension and value
  int pivot_dim;                                       //这个是关键
  double pivot_value;

  // The key for this node
  int key[3];

  // The value for this node
  double value;

  // The cluster label (leaf nodes)
  int cluster;

  // Child nodes
  struct pf_kdtree_node *children[2];

} pf_kdtree_node_t;         //结点

// A kd tree
typedef struct
{
  // Cell size
  double size[3];

  // The root node of the tree
  pf_kdtree_node_t *root;

  // The number of nodes in the tree
  int node_count, node_max_count;
  pf_kdtree_node_t *nodes;

  // The number of leaf nodes in the tree
  int leaf_count;

} pf_kdtree_t;            //树

//创建树  Create a tree 

pf_kdtree_t *pf_kdtree_alloc(int max_size)
{
  pf_kdtree_t *self;

  self = calloc(1, sizeof(pf_kdtree_t));     //    void *calloc(size_t n, size_t size);　　　void *malloc(size_t size);
                           //    calloc在动态分配完内存后，自动初始化该内存空间为零，而malloc不初始化，里面数据是随机的垃圾数据
  self->size[0] = 0.50;
  self->size[1] = 0.50;
  self->size[2] = (10 * M_PI / 180);

  self->root = NULL;

  self->node_count = 0;
  self->node_max_count = max_size;
  self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t));

  self->leaf_count = 0;

  return self;
}

//释放树　　Destroy a tree

// Destroy a tree
void pf_kdtree_free(pf_kdtree_t *self)
{
  free(self->nodes);
  free(self);
  return;
}

//清除所用结点

// Clear all entries from the tree
void pf_kdtree_clear(pf_kdtree_t *self)
{
  self->root = NULL;
  self->leaf_count = 0;
  self->node_count = 0;

  return;
}

//插入一个位姿到树中   Insert a pose into the tree

void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)                  
                                    //pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);    程序中的调用
{
  int key[3];

  key[0] = floor(pose.v[0] / self->size[0]);
  key[1] = floor(pose.v[1] / self->size[1]);
  key[2] = floor(pose.v[2] / self->size[2]);

  self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);             //　　将结点插入到树　　关键函数
  return;
}

//找到给定位姿的概率或权重

double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose)
{
  int key[3];
  pf_kdtree_node_t *node;

  key[0] = floor(pose.v[0] / self->size[0]);
  key[1] = floor(pose.v[1] / self->size[1]);
  key[2] = floor(pose.v[2] / self->size[2]);

  node = pf_kdtree_find_node(self, self->root, key);　　//　　在树中搜索结点
  if (node == NULL)
    return 0.0;
  return node->value;
}

//  cluster  是何物？？？？？？？？？？？

// Determine the cluster label for the given pose
int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
{
  int key[3];
  pf_kdtree_node_t *node;

  key[0] = floor(pose.v[0] / self->size[0]);
  key[1] = floor(pose.v[1] / self->size[1]);
  key[2] = floor(pose.v[2] / self->size[2]);

  node = pf_kdtree_find_node(self, self->root, key);
  if (node == NULL)
    return -1;
  return node->cluster;        // 后面再理解
}