rtcPoint3DKdTree.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2007
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: PUMA: Probablistic Unsupervised Model Acquisition
 * file .......: Point3DKdTree.cpp
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 08/29/2008
 * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
 * changed by .: $Author:benjaminpitzer $
 * revision ...: $Revision:141 $
 */

//== INCLUDES ==================================================================
#include "rtc/rtcPoint3DKdTree.h"

//==============================================================================
// NAMESPACE
namespace rtc {

Point3DKdTree::Point3DKdTree(float* x, float* y, float* z, int num_points)
{
  // Allocated memory for kd-Tree data
  dataPts = annAllocPts(num_points,3);
  for(int i = 0; i<num_points; ++i) {
    dataPts[i][0] = x[i];
    dataPts[i][1] = y[i];
    dataPts[i][2] = z[i];
  }

  // Actually build the tree
  ann_tree = new ANNkd_tree(dataPts, num_points, 3);
}

Point3DKdTree::~Point3DKdTree()
{
  delete ann_tree;
  annDeallocPts(dataPts);
  annClose();
}

int Point3DKdTree::findNearest(const Vec3f& p)
{
  return findNearest(p(0),p(1),p(2));
}

int Point3DKdTree::findNearest(float x, float y, float z)
{
  // build search vector
  ANNpoint queryPt = annAllocPt(3);
  queryPt[0] = x;
  queryPt[1] = y;
  queryPt[2] = z;
  // find closest ANN element
  ANNidx ann_index;
  ANNdist dist;
  // perform search in kd-tree
  ann_tree->annkSearch(queryPt, 1, &ann_index, &dist, 0.0);
  // dealloc point
  annDeallocPt(queryPt);
  // return the vertex handle
  return ann_index;
}

// find closest k points in kd-tree
void Point3DKdTree::findNearest(float x, float y, float z, int k, int* points)
{
  // build search vector
  ANNpoint queryPt=annAllocPt(3);
  queryPt[0] = x;
  queryPt[1] = y;
  queryPt[2] = z;
  // find closest ANN element
  ANNdist* dist = new ANNdist[k];
  // perform kd-tree search
  ann_tree->annkSearch(queryPt, k, points, dist, 0.0);
  // dealloc point
  annDeallocPt(queryPt);
  // free memory
  delete [] dist;
}

// find closest k points in kd-tree
void Point3DKdTree::findNearest(float x, float y, float z, int k, std::vector<int>& points)
{
  points.clear();
  // build search vector
  ANNpoint queryPt=annAllocPt(3);
  queryPt[0] = x;
  queryPt[1] = y;
  queryPt[2] = z;

  // find closest ANN element
  ANNidx* ann_index = new ANNidx[k];
  ANNdist* dist = new ANNdist[k];
  // perform kd-tree search
  ann_tree->annkSearch(queryPt, k, ann_index, dist, 0.0);
  // fill return vector
  for(int i=0;i<k; i++) points.push_back(ann_index[i]);
  // dealloc point
  annDeallocPt(queryPt);
  // free memory
  delete [] ann_index;
  delete [] dist;
}

void Point3DKdTree::findNearest(const Vec3f& p, int k, int* points)
{
  findNearest(p(0),p(1),p(2),k,points);
}

void Point3DKdTree::findNearest(const Vec3f& p, int k, std::vector<int>& points)
{
  findNearest(p(0),p(1),p(2),k,points);
}

int Point3DKdTree::findWithinRange(float x, float y, float z, float range, std::vector<int>& points)
{
  points.clear();
  // build search vector
  ANNpoint queryPt=annAllocPt(3);
  queryPt[0] = x;
  queryPt[1] = y;
  queryPt[2] = z;

  // first check how many points are within range
  int k = ann_tree->annkFRSearch(queryPt,rtc_sqr(range),0,NULL,NULL,0.0);
  // allocate memory
  ANNidx *index=new ANNidx[k];
  ANNdist *dist=new ANNdist[k];
  // find closest ANN elements within range
  k = ann_tree->annkFRSearch(queryPt,rtc_sqr(range),k,index,dist,0.0);
  // create vector of indices
  for(int i=0;i<k;++i)
    points.push_back(index[i]);
  // dealloc point
  annDeallocPt(queryPt);
  delete[] dist;
  delete[] index;
  return k;
}

int Point3DKdTree::findWithinRange(const Vec3f& p, float range, std::vector<int>& points)
{
  return findWithinRange(p[0],p[1],p[2],range,points);
}

void Point3DKdTree::findNearest(float* ann_sample, ANNidx* i,ANNdist* d)
{
  ANNpoint queryPt=annAllocPt(3);
  queryPt[0] = ann_sample[0];
  queryPt[1] = ann_sample[1];
  queryPt[2] = ann_sample[2];
  ann_tree->annkSearch(queryPt, 1, i, d, 0.0);
  // dealloc point
  annDeallocPt(queryPt);
}

//==============================================================================
} // NAMESPACE puma
//==============================================================================