3dpcp/.svn/pristine/50/50d7e0f8f910e2c877782cc6424b906100b34002.svn-base

86 lines
1.7 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* 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(double **_pts, int n)
{
pts = _pts;
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;
}
/**
* 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];
}