You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
130 lines
3.3 KiB
Plaintext
130 lines
3.3 KiB
Plaintext
/*
|
|
* kd implementation
|
|
*
|
|
* Copyright (C) Andreas Nuechter, Kai Lingemann, Thomas Escher
|
|
*
|
|
* Released under the GPL version 3.
|
|
*
|
|
*/
|
|
|
|
/** @file
|
|
* @brief An optimized k-d tree implementation
|
|
* @author Remus Dumitru. Jacobs University Bremen, Germany
|
|
* @author Corneliu-Claudiu Prodescu. Jacobs University Bremen, Germany
|
|
* @author Andreas Nuechter. Jacobs University Bremen, Germany.
|
|
* @author Kai Lingemann. Inst. of CS, University of Osnabrueck, Germany.
|
|
* @author Thomas Escher Inst. of CS, University of Osnabrueck, Germany.
|
|
*/
|
|
|
|
#ifdef _MSC_VER
|
|
#define _USE_MATH_DEFINES
|
|
#endif
|
|
|
|
#include "slam6d/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>
|
|
#include <limits>
|
|
#include <vector>
|
|
|
|
// KDtree class static variables
|
|
template<class PointData, class AccessorData, class AccessorFunc>
|
|
KDParams KDTreeImpl<PointData, AccessorData, AccessorFunc>::params[MAX_OPENMP_NUM_THREADS];
|
|
|
|
/**
|
|
* Constructor
|
|
*
|
|
* Create a KD tree from the points pointed to by the array pts
|
|
*
|
|
* @param pts 3D array of points
|
|
* @param n number of points
|
|
*/
|
|
KDtree::KDtree(double **pts, int n)
|
|
{
|
|
create(Void(), pts, n);
|
|
}
|
|
|
|
KDtree::~KDtree()
|
|
{
|
|
}
|
|
|
|
/**
|
|
* 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 *KDtree::FindClosest(double *_p,
|
|
double maxdist2,
|
|
int threadNum) const
|
|
{
|
|
params[threadNum].closest = 0;
|
|
params[threadNum].closest_d2 = maxdist2;
|
|
params[threadNum].p = _p;
|
|
_FindClosest(Void(), threadNum);
|
|
return params[threadNum].closest;
|
|
}
|
|
|
|
double *KDtree::FindClosestAlongDir(double *_p,
|
|
double *_dir,
|
|
double maxdist2,
|
|
int threadNum) const
|
|
{
|
|
params[threadNum].closest = NULL;
|
|
params[threadNum].closest_d2 = maxdist2;
|
|
params[threadNum].p = _p;
|
|
params[threadNum].dir = _dir;
|
|
_FindClosestAlongDir(Void(), threadNum);
|
|
return params[threadNum].closest;
|
|
}
|
|
|
|
vector<Point> KDtree::kNearestNeighbors(double *_p,
|
|
int k,
|
|
double sqRad2,
|
|
int threadNum) const
|
|
{
|
|
vector<Point> result;
|
|
params[threadNum].closest = 0;
|
|
params[threadNum].closest_d2 = sqRad2;
|
|
params[threadNum].p = _p;
|
|
params[threadNum].heap.clear();
|
|
_KNNSearch(Void(), threadNum);
|
|
|
|
while (k > 0 && params[threadNum].heap.empty() == false) {
|
|
Point pt = params[threadNum].heap.front().first;
|
|
result.push_back(pt);
|
|
std::pop_heap(params[threadNum].heap.begin(), params[threadNum].heap.end(), PointCompare());
|
|
params[threadNum].heap.pop_back();
|
|
k--;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
vector<Point> KDtree::fixedRangeSearch(double *_p,
|
|
double sqRad2,
|
|
int threadNum) const
|
|
{
|
|
vector<Point> result;
|
|
params[threadNum].closest = 0;
|
|
params[threadNum].closest_d2 = sqRad2;
|
|
params[threadNum].p = _p;
|
|
params[threadNum].heap.clear();
|
|
_FixedRangeSearch(Void(), threadNum);
|
|
|
|
for (vector<std::pair<Point, double> >::iterator it = params[threadNum].heap.begin(); it != params[threadNum].heap.end(); ++it) {
|
|
result.push_back(it->first);
|
|
}
|
|
|
|
return result;
|
|
}
|