Skip to content

Commit

Permalink
Added ability to pass one radius per point.
Browse files Browse the repository at this point in the history
  • Loading branch information
@stephanemagnenat
stephanemagnenat committed May 7, 2012
1 parent 5c692c0 commit 3dd9b9b
Show file tree
Hide file tree
Showing 6 changed files with 131 additions and 62 deletions.
53 changes: 27 additions & 26 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,32 +44,33 @@ find_path(EIGEN_INCLUDE_DIR Eigen/Core
)

# optionally, opencl
set(USE_OPEN_CL "false" CACHE BOOL "Set to ON to look for OpenCL")
if (USE_OPEN_CL)
find_path(OPENCL_INCLUDE_DIR CL/cl.h
/usr/local/include
/usr/include
)
if (WIN32)
find_library(OPENCL_LIBRARIES opencl64)
if (!OPENCL_LIBRARIES)
find_library(OPENCL_LIBRARIES opencl32)
endif (!OPENCL_LIBRARIES)
else (WIN32)
find_library(OPENCL_LIBRARIES OpenCL ENV LD_LIBRARY_PATH)
endif (WIN32)
if (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
add_definitions(-DHAVE_OPENCL)
set(EXTRA_LIBS ${OPENCL_LIBRARIES} ${EXTRA_LIBS})
include_directories(${OPENCL_INCLUDE_DIR})
add_definitions(-DOPENCL_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}/nabo/opencl/\")
message("OpenCL enabled and found, enabling CL support")
else (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
message("OpenCL enabled but not found, disabling CL support")
endif (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
else(USE_OPEN_CL)
message("OpenCL disabled, not looking for it")
endif(USE_OPEN_CL)
# OpenCL disabled as its code is not up-to-date with API
# set(USE_OPEN_CL "false" CACHE BOOL "Set to ON to look for OpenCL")
# if (USE_OPEN_CL)
# find_path(OPENCL_INCLUDE_DIR CL/cl.h
# /usr/local/include
# /usr/include
# )
# if (WIN32)
# find_library(OPENCL_LIBRARIES opencl64)
# if (!OPENCL_LIBRARIES)
# find_library(OPENCL_LIBRARIES opencl32)
# endif (!OPENCL_LIBRARIES)
# else (WIN32)
# find_library(OPENCL_LIBRARIES OpenCL ENV LD_LIBRARY_PATH)
# endif (WIN32)
# if (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
# add_definitions(-DHAVE_OPENCL)
# set(EXTRA_LIBS ${OPENCL_LIBRARIES} ${EXTRA_LIBS})
# include_directories(${OPENCL_INCLUDE_DIR})
# add_definitions(-DOPENCL_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}/nabo/opencl/\")
# message("OpenCL enabled and found, enabling CL support")
# else (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
# message("OpenCL enabled but not found, disabling CL support")
# endif (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
# else(USE_OPEN_CL)
# message("OpenCL disabled, not looking for it")
# endif(USE_OPEN_CL)

# include all libs so far
include_directories(${EIGEN_INCLUDE_DIR} ${Boost_INCLUDE_DIRS})
Expand Down
12 changes: 10 additions & 2 deletions nabo/brute_force_cpu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,17 +63,25 @@ namespace Nabo
template<typename T>
unsigned long BruteForceSearch<T>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
{
checkSizesKnn(query, indices, dists2, k);
const Vector maxRadii(Vector::Constant(query.cols(), maxRadius));
return knn(query, indices, dists2, maxRadii, k, epsilon, optionFlags);
}

template<typename T>
unsigned long BruteForceSearch<T>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k, const T epsilon, const unsigned optionFlags) const
{
checkSizesKnn(query, indices, dists2, k, &maxRadii);

const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
const bool sortResults(optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS);
const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);
const T maxRadius2(maxRadius * maxRadius);

IndexHeapSTL<Index, T> heap(k);

for (int c = 0; c < query.cols(); ++c)
{
const T maxRadius(maxRadii[c]);
const T maxRadius2(maxRadius * maxRadius);
const Vector& q(query.block(0,c,dim,1));
heap.reset();
for (int i = 0; i < this->cloud.cols(); ++i)
Expand Down
79 changes: 53 additions & 26 deletions nabo/kdtree_cpu.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,12 +266,8 @@ namespace Nabo
}

// create nodes
//nodes.resize(getTreeSize(cloud.cols()));
buildNodes(buildPoints.begin(), buildPoints.end(), minBound, maxBound);
buildPoints.clear();
//for (size_t i = 0; i < nodes.size(); ++i)
// cout << i << ": " << nodes[i].dim << " " << nodes[i].cutVal << " " << nodes[i].rightChild << endl;

}

template<typename T, typename Heap>
Expand All @@ -283,41 +279,72 @@ namespace Nabo
const bool sortResults(optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS);
const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);
const T maxRadius2(maxRadius * maxRadius);
assert(nodes.size() > 0);

assert(nodes.size() > 0);
Heap heap(k);
std::vector<T> off(dim, 0);

IndexMatrix result(k, query.cols());
const int colCount(query.cols());
unsigned long leafTouchedCount(0);
for (int i = 0; i < colCount; ++i)
{
leafTouchedCount += onePointKnn(query, indices, dists2, i, heap, off, 1+epsilon, maxRadius2, allowSelfMatch, collectStatistics, sortResults);
}
return leafTouchedCount;
}

template<typename T, typename Heap>
unsigned long KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k, const T epsilon, const unsigned optionFlags) const
{
checkSizesKnn(query, indices, dists2, k, &maxRadii);

const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
const bool sortResults(optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS);
const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);

assert(nodes.size() > 0);
Heap heap(k);
std::vector<T> off(dim, 0);

IndexMatrix result(k, query.cols());
const int colCount(query.cols());
unsigned long leafTouchedCount(0);
for (int i = 0; i < colCount; ++i)
{
fill(off.begin(), off.end(), 0);
heap.reset();

if (allowSelfMatch)
{
if (collectStatistics)
leafTouchedCount += recurseKnn<true, true>(&query.coeff(0, i), 0, 0, heap, off, 1+epsilon, maxRadius2);
else
recurseKnn<true, false>(&query.coeff(0, i), 0, 0, heap, off, 1+epsilon, maxRadius2);
}
const T maxRadius(maxRadii[i]);
const T maxRadius2(maxRadius * maxRadius);
leafTouchedCount += onePointKnn(query, indices, dists2, i, heap, off, 1+epsilon, maxRadius2, allowSelfMatch, collectStatistics, sortResults);
}
return leafTouchedCount;
}

template<typename T, typename Heap>
unsigned long KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::onePointKnn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, int i, Heap& heap, std::vector<T>& off, const T maxError, const T maxRadius2, const bool allowSelfMatch, const bool collectStatistics, const bool sortResults) const
{
fill(off.begin(), off.end(), 0);
heap.reset();
unsigned long leafTouchedCount(0);

if (allowSelfMatch)
{
if (collectStatistics)
leafTouchedCount += recurseKnn<true, true>(&query.coeff(0, i), 0, 0, heap, off, maxError, maxRadius2);
else
{
if (collectStatistics)
leafTouchedCount += recurseKnn<false, true>(&query.coeff(0, i), 0, 0, heap, off, 1+epsilon, maxRadius2);
else
recurseKnn<false, false>(&query.coeff(0, i), 0, 0, heap, off, 1+epsilon, maxRadius2);
}

if (sortResults)
heap.sort();

heap.getData(indices.col(i), dists2.col(i));
recurseKnn<true, false>(&query.coeff(0, i), 0, 0, heap, off, maxError, maxRadius2);
}
else
{
if (collectStatistics)
leafTouchedCount += recurseKnn<false, true>(&query.coeff(0, i), 0, 0, heap, off, maxError, maxRadius2);
else
recurseKnn<false, false>(&query.coeff(0, i), 0, 0, heap, off, maxError, maxRadius2);
}

if (sortResults)
heap.sort();

heap.getData(indices.col(i), dists2.col(i));
return leafTouchedCount;
}

Expand Down
12 changes: 7 additions & 5 deletions nabo/nabo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,18 +78,20 @@ namespace Nabo
}

template<typename T>
void NearestNeighbourSearch<T>::checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k) const
void NearestNeighbourSearch<T>::checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k, const Vector* maxRadii) const
{
if (query.rows() < dim)
throw runtime_error((boost::format("Query has less dimensions (%1%) than requested for cloud (%2%)") % query.rows() % dim).str());
if (indices.rows() != k)
throw runtime_error((boost::format("Index matrix has less rows (%1%) than k (%2%)") % indices.rows() % k).str());
throw runtime_error((boost::format("Index matrix has a different number of rows (%1%) than k (%2%)") % indices.rows() % k).str());
if (indices.cols() != query.cols())
throw runtime_error((boost::format("Index matrix has less columns (%1%) than query (%2%)") % indices.rows() % query.cols()).str());
throw runtime_error((boost::format("Index matrix has a different number of columns (%1%) than query (%2%)") % indices.rows() % query.cols()).str());
if (dists2.rows() != k)
throw runtime_error((boost::format("Distance matrix has less rows (%1%) than k (%2%)") % dists2.rows() % k).str());
throw runtime_error((boost::format("Distance matrix has a different number of rows (%1%) than k (%2%)") % dists2.rows() % k).str());
if (dists2.cols() != query.cols())
throw runtime_error((boost::format("Distance matrix has less columns (%1%) than query (%2%)") % dists2.rows() % query.cols()).str());
throw runtime_error((boost::format("Distance matrix has a different number of columns (%1%) than query (%2%)") % dists2.rows() % query.cols()).str());
if (maxRadii && (maxRadii->size() != query.cols()))
throw runtime_error((boost::format("Maximum radii vector has not the same length (%1%) than query has columns (%2%)") % maxRadii->size() % k).str());
}


Expand Down
20 changes: 17 additions & 3 deletions nabo/nabo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,7 +278,7 @@ namespace Nabo
};

//! Find the k nearest neighbours of query
/*! If the search finds less than k points, the empty entries in dists2 will be filled with infinity and the indices with 0.
/*! If the search finds less than k points, the empty entries in dists2 will be filled with infinity and the indices with 0. If you must query more than one point at once, use the version of the knn() function taking matrices as input, because it is much faster.
* \param query query point
* \param indices indices of nearest neighbours, must be of size k
* \param dists2 squared distances to nearest neighbours, must be of size k
Expand All @@ -303,6 +303,19 @@ namespace Nabo
*/
virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0, const T maxRadius = std::numeric_limits<T>::infinity()) const = 0;

//! Find the k nearest neighbours for each point of query
/*! If the search finds less than k points, the empty entries in dists2 will be filled with infinity and the indices with 0.
* \param query query points
* \param indices indices of nearest neighbours, must be of size k x query.cols()
* \param dists2 squared distances to nearest neighbours, must be of size k x query.cols()
* \param maxRadii vector of maximum radii in which to search, used to prune search, is not affected by epsilon
* \param k number of nearest neighbour requested
* \param epsilon maximal percentage of error for approximate search, 0 for exact search
* \param optionFlags search options, a bitwise OR of elements of SearchOptionFlags
* \return if creationOptionFlags contains TOUCH_STATISTICS, return the number of point touched, otherwise return 0
*/
virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const = 0;

//! Create a nearest-neighbour search
/*! \param cloud data-point cloud in which to search
* \param dim number of dimensions to consider, must be lower or equal to cloud.rows()
Expand Down Expand Up @@ -349,8 +362,9 @@ namespace Nabo
/*! \param query query points
* \param k number of nearest neighbour requested
* \param indices indices of nearest neighbours, must be of size k x query.cols()
* \param dists2 squared distances to nearest neighbours, must be of size k x query.cols() */
void checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k) const;
* \param dists2 squared distances to nearest neighbours, must be of size k x query.cols()
\param maxRadii if non 0, maximum radii, must be of size k */
void checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k, const Vector* maxRadii = 0) const;
};

// Convenience typedefs
Expand Down
17 changes: 17 additions & 0 deletions nabo/nabo_private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,7 @@ namespace Nabo
//! constructor, calls NearestNeighbourSearch<T>(cloud)
BruteForceSearch(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags);
virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
};

//! KDTree, unbalanced, points in leaves, stack, implicit bounds, ANN_KD_SL_MIDPT, optimised implementation
Expand Down Expand Up @@ -184,6 +185,21 @@ namespace Nabo
//! construct nodes for points [first..last[ inside the hyperrectangle [minValues..maxValues]
unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues);

//! search one point, call recurseKnn with the correct template parameters
/** \param query pointer to query coordinates
* \param indices indices of nearest neighbours, must be of size k x query.cols()
* \param dists2 squared distances to nearest neighbours, must be of size k x query.cols()
* \param i index of point to search
* \param heap reference to heap
* \param off reference to array of offsets
* \param maxError error factor (1 + epsilon)
* \param maxRadius2 square of maximum radius
* \param allowSelfMatch whether to allow self match
* \param collectStatistics whether to collect statistics
* \param sortResults wether to sort results
*/
unsigned long onePointKnn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, int i, Heap& heap, std::vector<T>& off, const T maxError, const T maxRadius2, const bool allowSelfMatch, const bool collectStatistics, const bool sortResults) const;

//! recursive search, strongly inspired by ANN and [Arya & Mount, Algorithms for fast vector quantization, 1993]
/** \param query pointer to query coordinates
* \param n index of node to visit
Expand All @@ -200,6 +216,7 @@ namespace Nabo
//! constructor, calls NearestNeighbourSearch<T>(cloud)
KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters);
virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
};

#ifdef HAVE_OPENCL
Expand Down

0 comments on commit 3dd9b9b

Please sign in to comment.