kdtree_common.cc

Go to the documentation of this file.

// Guy Shechter
// June 2004

// Variables used for time debugging.
//TV tv1,tv2;
//TZ tz;

#include "icp/kdtree_common.h"
#include <stdlib.h>
#include <stdio.h>
#include <cmath>

namespace icp {

int swap_tmp_int;
#define INT_SWAP(X,Y)  { swap_tmp_int=X; X=Y; Y=swap_tmp_int; }

//
// Call this function to build the k-d Tree.
//
Tree *build_kdtree(double *reference, int N, int D, int *index, 
                   int L,int offset) {

  Tree *treeptr;
  if ( (treeptr = (Tree *) malloc(sizeof(Tree))) == NULL )
    fprintf(stderr,"Error allocating memory for kD-Tree\n");
  
  treeptr->rootptr = build_kdtree_core(reference, N, D, index, L, offset);
  treeptr->dims = D;

  return treeptr;
}

//
// This is the core function for building the k-D tree for raw input data.
//
Node *build_kdtree_core(double *reference, int N, int D, int *index, 
                        int L,int offset) {
  int i,median;
  Node *nodeptr;
  
#ifdef DEBUG_BUILD_TREE
  fprintf(stderr,"Entering build_kdtree \n");
#endif
  
  if ( (nodeptr = (Node *) malloc(sizeof(Node))) == NULL )
    fprintf(stderr,"Error allocating memory for kD-Tree\n");

  if ( (nodeptr->pt = (double*) malloc(sizeof(double)*D)) == NULL )
    fprintf(stderr,"Error allocating memory for kD-Tree\n");
  
  if (L==1) {
    for (i=0; i < D; i++)
      nodeptr->pt[i] = EVAL_INDEX(index[0],i,N);
    nodeptr->index = (unsigned int) index[0];
    
#ifdef DEBUG_BUILD_TREE
    fprintf(stderr,"Created leaf     node, (dim=%d) (value: ", offset);
    for (i=0; i < D; i++)
      fprintf(stderr," %f",nodeptr->pt[i]);
    fprintf(stderr," )\n");
#endif
    
    nodeptr->orientation = offset;
    nodeptr->left=NULL;
    nodeptr->right=NULL;
    return nodeptr;
  }
  
  quicksort(index,0,L-1,reference,offset,N);
  median = (int)((L)/2.0);
  
  for (i=0; i < D; i++)
    nodeptr->pt[i] = EVAL_INDEX(index[median],i,N);
  nodeptr->index = (unsigned int) index[median];
  nodeptr->orientation = offset;
  nodeptr->left=NULL;
  nodeptr->right=NULL;
  
#ifdef DEBUG_BUILD_TREE
  fprintf(stderr,"Created internal node, (dim=%d) (value: ", offset);
  for (i=0; i < D; i++)
    fprintf(stderr," %f",nodeptr->pt[i]);
  fprintf(stderr," )\n");
#endif
  
  nodeptr->left = build_kdtree_core(reference,N,D,index,median,(offset+1)%D);
  
  if (L-median-1)
    nodeptr->right = build_kdtree_core(reference,N,D,index+median+1,
                                       L-median-1,(offset+1)%D);
  
  return nodeptr;
}


// 
//   The following two functions are for the quicksort algorithm 
//
int partition(int *a, int p, int r, double *reference, int offset, int D){
  int i,j;
  double x;
  
  x= EVAL_INDEX(a[p],offset,D); i=p-1; j=r+1;
  while (1) {
    for (j--; EVAL_INDEX(a[j],offset,D) > x; j--);
    for (i++; EVAL_INDEX(a[i],offset,D) <x; i++);
    if (i<j){
      INT_SWAP(a[j],a[i]);
    }
    else return j;
  }
}

void quicksort(int *ra, int p, int r, double *reference, int offset,int D){
  int q;
  
  if (p < r) {
    q = partition(ra,p,r,reference,offset,D);
    quicksort(ra,p,q,reference,offset,D);
    quicksort(ra,q+1,r,reference,offset,D);
  }
}



//
// Deallocate memory used by the k-D tree
//
void free_tree(Node *pVertex) {
  
  if (pVertex == NULL)
    return;
  if (pVertex->left)  free_tree(pVertex->left);
  if (pVertex->right) free_tree(pVertex->right);
  
  free(pVertex->pt);
  free(pVertex);
  return;
}


//
// Compute the norm distance between two points of dimension Dim
//
double calcdistance(double *pt1, double *pt2, int Dim){
  int j;
  double d=0;
  for (j=0; j < Dim; j++) 
    d+= (pt1[j]-pt2[j])*(pt1[j]-pt2[j]);
  return (sqrt(d));
}


Node *pointLocation(Node *v, double *pt,int D){
  
  //if (!v) v=root;

  if ( ( v->left == NULL) && (v->right == NULL) ) 
    return v;

  if (    pt[v->orientation] < v->pt[ v->orientation] )
    return ( (v->left) ? pointLocation(v->left,pt,D) : v);
  else if (    pt[v->orientation] > v->pt[v->orientation]  ) 
    return ( (v->right) ? pointLocation(v->right,pt,D): v);
  else {
    //What we have is pt[v->orientation] == v->pt[v->orientation] 
    Node *vl = ( (v->left)? pointLocation(v->left,pt,D) : v);
    Node *vr = ( (v->right)? pointLocation(v->right,pt,D) : v);
    if ( calcdistance(vl->pt,pt,D) < calcdistance(vr->pt,pt,D) )
      return vl;
    else
      return vr;
  }
}


Node * rangeQuery(Node *v, double distance, double *pt, int D){

  Node *current=NULL, *tmp;
  int j;

  
  if ( ( (v->pt[v->orientation] - distance) <=  pt[v->orientation]) &&
       ( (v->pt[v->orientation] + distance) >=  pt[v->orientation]) ){
    if (calcdistance(pt,v->pt,D) <= distance){
      current=(Node*) malloc(sizeof(Node));
      current->pt=(double*) malloc(sizeof(double)*D);
      current->right=NULL;
      for (j=0; j < D; j++) current->pt[j]=v->pt[j];
      current->index=v->index;
    }
  }

  
  if (  (!(v->left)) && (!(v->right)) ) 
    return current;
  
  tmp=current;
  while (tmp && tmp->right) 
    tmp=tmp->right;
  
  if (v->left){
    if ( v->pt[v->orientation] >= (pt[v->orientation] - distance ) ){
      if (tmp)
        tmp->right=rangeQuery(v->left, distance, pt,D);
      else
        current=rangeQuery(v->left, distance, pt, D);
    }
  }
  
  tmp=current;
  while (tmp &&tmp->right) 
    tmp=tmp->right;
  
  if (v->right){
    if (   v->pt[v->orientation]  <= (pt[v->orientation] + distance ) ){
      if (tmp)
        tmp->right=rangeQuery(v->right, distance, pt, D);
      else
        current=   rangeQuery(v->right, distance, pt, D);
    }
  }    

  return current;
}

//
// The top-level function for running a query on the k-D tree.
//
void run_queries( Node *pVertex, double *model, int M, int D, 
                  double *closest_pt, double *distance, short ReturnType) {
  
  int i,j;
  double min_distance, *pt;
  Node *LL, *cur, *leaf, *tmp;
  
  pt= (double*)malloc(sizeof(double)*D);
  
  for (i=0; i < M; i++) {
    
#ifdef DEBUG_RUN_QUERIES
    fprintf(stderr,"Running Query (%d/%d) (value: ",
              i+1, M);
    for (j=0; j < D ; j++)
      fprintf(stderr," %f", model[ M*j+i]);
    fprintf(stderr," )\n");
#endif
    
    for (j=0; j < D; j++) 
      pt[j]=model[M*j+i];
    
    leaf=pointLocation(pVertex,pt,D);
    min_distance=calcdistance(leaf->pt, pt,D )+0.001;

    LL=rangeQuery(pVertex,min_distance,pt,D);

    if (!LL) {
      if (ReturnType == RETURN_INDEX) 
        closest_pt[i] = -1;
      else{
        for (j=0; j< D; j++)
          closest_pt[j*M+i]=-1;
      }
      fprintf(stderr,"Null LL\n");
    } 
    else {
      distance[i]=calcdistance(LL->pt, pt,D);
      if (ReturnType == RETURN_INDEX) 
        closest_pt[i] = LL->index;
      else {
        for (j=0; j < D; j++) 
          closest_pt[j*M+i] = LL->pt[j];
      }
      cur=LL;
      while (cur){
        if ( calcdistance(cur->pt, pt,D) <= distance[i] ) {
          if (ReturnType == RETURN_INDEX) 
            closest_pt[i] = cur->index;
          else {
            for (j=0; j < D; j++) 
              closest_pt[j*M+i] = cur->pt[j];
          }
          distance[i]=calcdistance(cur->pt, pt,D);
        }
        tmp=cur;
        cur=cur->right;
        free(tmp->pt);
        free(tmp);
      }
    }

#ifdef DEBUG_RUN_QUERIES
    fprintf(stderr,"Distance to closest point is %f\n",distance[i]);
#endif

  }
  free(pt);
}



// 
// Outputs the k-D tree while performing a depth first traversal.
//
void display_tree(Node *nodeptr,int D) {
  int i;
  
  fprintf(stderr,"Node: (dim = %d) (idx= %u) (value = ", nodeptr->orientation,
            nodeptr->index);
  for (i=0; i <D; i++)
    fprintf(stderr,"%f\t",nodeptr->pt[i]);
  fprintf(stderr,")\n");

  if (nodeptr->left == NULL) 
    fprintf(stderr,"Left is (null)\n");
  else{
    fprintf(stderr,"Going left\n");
    display_tree(nodeptr->left,D);
  }
  
  if (nodeptr->right == NULL) 
    fprintf(stderr,"Right is (null)\n");
  else{
    fprintf(stderr,"Going right\n");
    display_tree(nodeptr->right,D);
  }
}









//
// The top-level function for running a range search on the k-D tree.
//
void run_range_search( Node *pVertex, double *model, int M, int D, 
                       double distlim, double **pts_in_range, 
                       unsigned int *L, unsigned int **indices){
  
  int i,j, count;
  double *pt;
  Node *LL, *cur, *tmp;

  pt= (double*)malloc(sizeof(double)*D);

  for (i=0; i < M; i++) {

#ifdef DEBUG_RUN_RANGE_SEARCH
    fprintf(stderr,"Running Search (%d/%d) (value: ",
              i+1, M);
    for (j=0; j < D ; j++)
      fprintf(stderr," %f", model[ M*j+i]);
    fprintf(stderr," )\n");
#endif

    for (j=0; j < D ; j++)
      pt[j]=model[M*j+i];
    LL=rangeQuery(pVertex,distlim,pt,D);

    if (!LL) {
      *L=0;
      (*pts_in_range)=NULL;
#ifdef DEBUG_RUN_RANGE_SEARCH
      fprintf(stderr,"Null LL\n");
#endif
    } 
    else {
      cur=LL;
      count=0;
      while (cur){
        cur=cur->right; count++;
      }
#ifdef DEBUG_RUN_RANGE_SEARCH
      fprintf(stderr,"Found %d points\n",count);
#endif
      *L=count;
      (*indices) = (unsigned int *) malloc (sizeof(unsigned int)*count);
      (*pts_in_range) = (double *) malloc (sizeof(double) *count*D);


      cur=LL;
      count=0;
      while(cur){
        (*indices)[count] = cur->index;
        for (j=0; j < D; j++) {
          (*pts_in_range)[j*(*L)+count] = cur->pt[j];
        }
        tmp=cur;
        cur=cur->right;
        free(tmp->pt);
        free(tmp);
        count++;
      }
    }
  }
  free(pt);
}

}