rtcPoint3DKdTree.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2007
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: PUMA: Probablistic Unsupervised Model Acquisition
00011  * file .......: Point3DKdTree.cpp
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/29/2008
00015  * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
00016  * changed by .: $Author:benjaminpitzer $
00017  * revision ...: $Revision:141 $
00018  */
00019 
00020 //== INCLUDES ==================================================================
00021 #include "rtc/rtcPoint3DKdTree.h"
00022 
00023 //==============================================================================
00024 // NAMESPACE
00025 namespace rtc {
00026 
00027 Point3DKdTree::Point3DKdTree(float* x, float* y, float* z, int num_points)
00028 {
00029   // Allocated memory for kd-Tree data
00030   dataPts = annAllocPts(num_points,3);
00031   for(int i = 0; i<num_points; ++i) {
00032     dataPts[i][0] = x[i];
00033     dataPts[i][1] = y[i];
00034     dataPts[i][2] = z[i];
00035   }
00036 
00037   // Actually build the tree
00038   ann_tree = new ANNkd_tree(dataPts, num_points, 3);
00039 }
00040 
00041 Point3DKdTree::~Point3DKdTree()
00042 {
00043   delete ann_tree;
00044   annDeallocPts(dataPts);
00045   annClose();
00046 }
00047 
00048 int Point3DKdTree::findNearest(const Vec3f& p)
00049 {
00050   return findNearest(p(0),p(1),p(2));
00051 }
00052 
00053 int Point3DKdTree::findNearest(float x, float y, float z)
00054 {
00055   // build search vector
00056   ANNpoint queryPt = annAllocPt(3);
00057   queryPt[0] = x;
00058   queryPt[1] = y;
00059   queryPt[2] = z;
00060   // find closest ANN element
00061   ANNidx ann_index;
00062   ANNdist dist;
00063   // perform search in kd-tree
00064   ann_tree->annkSearch(queryPt, 1, &ann_index, &dist, 0.0);
00065   // dealloc point
00066   annDeallocPt(queryPt);
00067   // return the vertex handle
00068   return ann_index;
00069 }
00070 
00071 // find closest k points in kd-tree
00072 void Point3DKdTree::findNearest(float x, float y, float z, int k, int* points)
00073 {
00074   // build search vector
00075   ANNpoint queryPt=annAllocPt(3);
00076   queryPt[0] = x;
00077   queryPt[1] = y;
00078   queryPt[2] = z;
00079   // find closest ANN element
00080   ANNdist* dist = new ANNdist[k];
00081   // perform kd-tree search
00082   ann_tree->annkSearch(queryPt, k, points, dist, 0.0);
00083   // dealloc point
00084   annDeallocPt(queryPt);
00085   // free memory
00086   delete [] dist;
00087 }
00088 
00089 // find closest k points in kd-tree
00090 void Point3DKdTree::findNearest(float x, float y, float z, int k, std::vector<int>& points)
00091 {
00092   points.clear();
00093   // build search vector
00094   ANNpoint queryPt=annAllocPt(3);
00095   queryPt[0] = x;
00096   queryPt[1] = y;
00097   queryPt[2] = z;
00098 
00099   // find closest ANN element
00100   ANNidx* ann_index = new ANNidx[k];
00101   ANNdist* dist = new ANNdist[k];
00102   // perform kd-tree search
00103   ann_tree->annkSearch(queryPt, k, ann_index, dist, 0.0);
00104   // fill return vector
00105   for(int i=0;i<k; i++) points.push_back(ann_index[i]);
00106   // dealloc point
00107   annDeallocPt(queryPt);
00108   // free memory
00109   delete [] ann_index;
00110   delete [] dist;
00111 }
00112 
00113 void Point3DKdTree::findNearest(const Vec3f& p, int k, int* points)
00114 {
00115   findNearest(p(0),p(1),p(2),k,points);
00116 }
00117 
00118 void Point3DKdTree::findNearest(const Vec3f& p, int k, std::vector<int>& points)
00119 {
00120   findNearest(p(0),p(1),p(2),k,points);
00121 }
00122 
00123 int Point3DKdTree::findWithinRange(float x, float y, float z, float range, std::vector<int>& points)
00124 {
00125   points.clear();
00126   // build search vector
00127   ANNpoint queryPt=annAllocPt(3);
00128   queryPt[0] = x;
00129   queryPt[1] = y;
00130   queryPt[2] = z;
00131 
00132   // first check how many points are within range
00133   int k = ann_tree->annkFRSearch(queryPt,rtc_sqr(range),0,NULL,NULL,0.0);
00134   // allocate memory
00135   ANNidx *index=new ANNidx[k];
00136   ANNdist *dist=new ANNdist[k];
00137   // find closest ANN elements within range
00138   k = ann_tree->annkFRSearch(queryPt,rtc_sqr(range),k,index,dist,0.0);
00139   // create vector of indices
00140   for(int i=0;i<k;++i)
00141     points.push_back(index[i]);
00142   // dealloc point
00143   annDeallocPt(queryPt);
00144   delete[] dist;
00145   delete[] index;
00146   return k;
00147 }
00148 
00149 int Point3DKdTree::findWithinRange(const Vec3f& p, float range, std::vector<int>& points)
00150 {
00151   return findWithinRange(p[0],p[1],p[2],range,points);
00152 }
00153 
00154 void Point3DKdTree::findNearest(float* ann_sample, ANNidx* i,ANNdist* d)
00155 {
00156   ANNpoint queryPt=annAllocPt(3);
00157   queryPt[0] = ann_sample[0];
00158   queryPt[1] = ann_sample[1];
00159   queryPt[2] = ann_sample[2];
00160   ann_tree->annkSearch(queryPt, 1, i, d, 0.0);
00161   // dealloc point
00162   annDeallocPt(queryPt);
00163 }
00164 
00165 //==============================================================================
00166 } // NAMESPACE puma
00167 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Mon Oct 6 2014 10:07:35