3dpcp/.svn/pristine/98/98c86c975579fc3529238a3892d5ae76a563a763.svn-base
2012-10-24 11:28:22 +02:00

105 lines
2.2 KiB
Text

/*
* ann_kd implementation
*
* Copyright (C) Andreas Nuechter, Ulugbek Makhmudov
*
* Released under the GPL version 3.
*
*/
/** @file
* @brief Encapsules the implementation of ANN k-d trees.
* @author Ulugbek Makhmudov, Jacobs University Bremen, Bremen, Germany.
* @author Andreas Nuechter, Jacobs University Bremen, Bremen, Germany.
*/
#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif
#include "slam6d/ann_kd.h"
#include "slam6d/globals.icc"
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;
#include <algorithm>
using std::swap;
#include <cmath>
#include <cstring>
/**
* Constructor
*
* Create ANN KD tree from the points pointed to by the array pts
*
* @param pts 3D array of points
* @param n number of points
*/
ANNtree::ANNtree(PointerArray<double>&_pts, int n)
{
/*
pts = new double*[n];
for(unsigned int i = 0; i < n; i++) {
pts[i] = new double[3];
pts[i][0] = _pts.get()[i][0];
pts[i][1] = _pts.get()[i][1];
pts[i][2] = _pts.get()[i][2];
}
*/
pts = new double*[n];
double* tpts = new double[3*n];
for(unsigned int i = 0, j = 0; i < n; i++) {
pts[i] = &tpts[j];
tpts[j++] = _pts.get()[i][0];
tpts[j++] = _pts.get()[i][1];
tpts[j++] = _pts.get()[i][2];
}
annkd = new ANNkd_tree(pts, n, 3, 1, ANN_KD_SUGGEST); // links to the constructor of ANNkd_tree
cout << "ANNkd_tree was generated with " << n << " points" << endl;
nn = new ANNdist[1];
nn_idx = new ANNidx[1];
}
/**
* Destructor
*
* Cleans up the instance of ANN KD tree
*
*
*/
ANNtree::~ANNtree()
{
delete annkd; //links to the destructor of ANNkd_tree
delete [] nn;
delete [] nn_idx;
delete [] pts[0];
delete [] pts;
}
/**
* Finds the closest point within the tree,
* wrt. the point given as first parameter.
* @param _p point
* @param maxdist2 maximal search distance.
* @param threadNum Thread number, for parallelization
* @return Pointer to the closest point
*/
double *ANNtree::FindClosest(double *_p, double maxdist2, int threadNum) const
{
#pragma omp critical
annkd->annkSearch(_p, 1, nn_idx, nn, 0.0);
int idx = nn_idx[0];
if (Dist2(_p, pts[idx]) > maxdist2) return 0;
return pts[idx];
}