00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "nabo.h"
00033 #include "nabo_private.h"
00034 #include "index_heap.h"
00035 #include <limits>
00036 #include <algorithm>
00037 #include <stdexcept>
00038 #include <boost/format.hpp>
00039
00045 namespace Nabo
00046 {
00047 using namespace std;
00048
00049 template<typename T, typename CloudType>
00050 NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags):
00051 cloud(cloud),
00052 dim(min(dim, int(cloud.rows()))),
00053 creationOptionFlags(creationOptionFlags),
00054 minBound(Vector::Constant(this->dim, numeric_limits<T>::max())),
00055 maxBound(Vector::Constant(this->dim, numeric_limits<T>::min()))
00056 {
00057 if (cloud.cols() == 0)
00058 throw runtime_error("Cloud has no points");
00059 if (cloud.rows() == 0)
00060 throw runtime_error("Cloud has 0 dimensions");
00061 }
00062
00063 template<typename T, typename CloudType>
00064 unsigned long NearestNeighbourSearch<T, CloudType>::knn(const Vector& query, IndexVector& indices, Vector& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
00065 {
00066 #ifdef EIGEN3_API
00067 const Eigen::Map<const Matrix> queryMatrix(&query.coeff(0,0), dim, 1);
00068 #else // EIGEN3_API
00069 const Eigen::Map<Matrix> queryMatrix(&query.coeff(0,0), dim, 1);
00070 #endif // EIGEN3_API
00071
00072
00073
00074
00075 IndexMatrix indexMatrix(k, 1);
00076 Matrix dists2Matrix(k, 1);
00077 const unsigned long stats = knn(queryMatrix, indexMatrix, dists2Matrix, k, epsilon, optionFlags, maxRadius);
00078 indices = indexMatrix.col(0);
00079 dists2 = dists2Matrix.col(0);
00080 return stats;
00081 }
00082
00083 template<typename T, typename CloudType>
00084 void NearestNeighbourSearch<T, CloudType>::checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k, const unsigned optionFlags, const Vector* maxRadii) const
00085 {
00086 const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T, CloudType>::ALLOW_SELF_MATCH);
00087 if (allowSelfMatch)
00088 {
00089 if (k > cloud.cols())
00090 throw runtime_error((boost::format("Requesting more points (%1%) than available in cloud (%2%)") % k % cloud.cols()).str());
00091 }
00092 else
00093 {
00094 if (k > cloud.cols()-1)
00095 throw runtime_error((boost::format("Requesting more points (%1%) than available in cloud minus 1 (%2%) (as self match is forbidden)") % k % (cloud.cols()-1)).str());
00096 }
00097 if (query.rows() < dim)
00098 throw runtime_error((boost::format("Query has less dimensions (%1%) than requested for cloud (%2%)") % query.rows() % dim).str());
00099 if (indices.rows() != k)
00100 throw runtime_error((boost::format("Index matrix has a different number of rows (%1%) than k (%2%)") % indices.rows() % k).str());
00101 if (indices.cols() != query.cols())
00102 throw runtime_error((boost::format("Index matrix has a different number of columns (%1%) than query (%2%)") % indices.rows() % query.cols()).str());
00103 if (dists2.rows() != k)
00104 throw runtime_error((boost::format("Distance matrix has a different number of rows (%1%) than k (%2%)") % dists2.rows() % k).str());
00105 if (dists2.cols() != query.cols())
00106 throw runtime_error((boost::format("Distance matrix has a different number of columns (%1%) than query (%2%)") % dists2.rows() % query.cols()).str());
00107 if (maxRadii && (maxRadii->size() != query.cols()))
00108 throw runtime_error((boost::format("Maximum radii vector has not the same length (%1%) than query has columns (%2%)") % maxRadii->size() % k).str());
00109 const unsigned maxOptionFlagsValue(ALLOW_SELF_MATCH|SORT_RESULTS);
00110 if (optionFlags > maxOptionFlagsValue)
00111 throw runtime_error((boost::format("OR-ed value of option flags (%1%) is larger than maximal valid value (%2%)") % optionFlags % maxOptionFlagsValue).str());
00112 }
00113
00114
00115 template<typename T, typename CloudType>
00116 NearestNeighbourSearch<T, CloudType>* NearestNeighbourSearch<T, CloudType>::create(const CloudType& cloud, const Index dim, const SearchType preferedType, const unsigned creationOptionFlags, const Parameters& additionalParameters)
00117 {
00118 if (dim <= 0)
00119 throw runtime_error("Your space must have at least one dimension");
00120 switch (preferedType)
00121 {
00122 case BRUTE_FORCE: return new BruteForceSearch<T, CloudType>(cloud, dim, creationOptionFlags);
00123 case KDTREE_LINEAR_HEAP: return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapBruteForceVector<int,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
00124 case KDTREE_TREE_HEAP: return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapSTL<int,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
00125 #ifdef HAVE_OPENCL
00126 case KDTREE_CL_PT_IN_NODES: return new KDTreeBalancedPtInNodesStackOpenCL<T, CloudType>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
00127 case KDTREE_CL_PT_IN_LEAVES: return new KDTreeBalancedPtInLeavesStackOpenCL<T, CloudType>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
00128 case BRUTE_FORCE_CL: return new BruteForceSearchOpenCL<T, CloudType>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
00129 #else // HAVE_OPENCL
00130 case KDTREE_CL_PT_IN_NODES: throw runtime_error("OpenCL not found during compilation");
00131 case KDTREE_CL_PT_IN_LEAVES: throw runtime_error("OpenCL not found during compilation");
00132 case BRUTE_FORCE_CL: throw runtime_error("OpenCL not found during compilation");
00133 #endif // HAVE_OPENCL
00134 default: throw runtime_error("Unknown search type");
00135 }
00136 }
00137
00138 template<typename T, typename CloudType>
00139 NearestNeighbourSearch<T, CloudType>* NearestNeighbourSearch<T, CloudType>::createBruteForce(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags)
00140 {
00141 if (dim <= 0)
00142 throw runtime_error("Your space must have at least one dimension");
00143 return new BruteForceSearch<T, CloudType>(cloud, dim, creationOptionFlags);
00144 }
00145
00146 template<typename T, typename CloudType>
00147 NearestNeighbourSearch<T, CloudType>* NearestNeighbourSearch<T, CloudType>::createKDTreeLinearHeap(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters)
00148 {
00149 if (dim <= 0)
00150 throw runtime_error("Your space must have at least one dimension");
00151 return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapBruteForceVector<int,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
00152 }
00153
00154 template<typename T, typename CloudType>
00155 NearestNeighbourSearch<T, CloudType>* NearestNeighbourSearch<T, CloudType>::createKDTreeTreeHeap(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters)
00156 {
00157 if (dim <= 0)
00158 throw runtime_error("Your space must have at least one dimension");
00159 return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapSTL<int,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
00160 }
00161
00162 template struct NearestNeighbourSearch<float>;
00163 template struct NearestNeighbourSearch<double>;
00164 template struct NearestNeighbourSearch<float, Eigen::Matrix3Xf>;
00165 template struct NearestNeighbourSearch<double, Eigen::Matrix3Xd>;
00166 template struct NearestNeighbourSearch<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
00167 template struct NearestNeighbourSearch<double, Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
00168 }