nabo/nabo.cpp
Go to the documentation of this file.
1 /*
2 
3 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
4 You can contact the author at <stephane at magnenat dot net>
5 
6 All rights reserved.
7 
8 Redistribution and use in source and binary forms, with or without
9 modification, are permitted provided that the following conditions are met:
10  * Redistributions of source code must retain the above copyright
11  notice, this list of conditions and the following disclaimer.
12  * Redistributions in binary form must reproduce the above copyright
13  notice, this list of conditions and the following disclaimer in the
14  documentation and/or other materials provided with the distribution.
15  * Neither the name of the <organization> nor the
16  names of its contributors may be used to endorse or promote products
17  derived from this software without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
23 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
28 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 
30 */
31 
32 #include "nabo.h"
33 #include "nabo_private.h"
34 #include "index_heap.h"
35 #include <limits>
36 #include <algorithm>
37 #include <stdexcept>
38 #include <sstream>
39 
45 namespace Nabo
46 {
47  using namespace std;
48 
49  // Custom exception class supporting stream-style message constructions.
50  struct runtime_error : std::runtime_error {
51  stringstream ss;
52 
53  template<typename T> runtime_error& operator<<(const T& t) {
54  ss << t;
55 
56  // Linux executor would not print correctly by overiding "virtual const char* what()".
57  // One solution is refreshing underlying std::runtime_error every time message is changed.
58  static_cast<std::runtime_error&>(*this) = std::runtime_error(ss.str());
59 
60  return *this;
61  }
62 
64  runtime_error(const runtime_error& re): std::runtime_error(re.ss.str()), ss(re.ss.str()) {}
65  virtual ~runtime_error() throw () {}
66  };
67 
68  template<typename T, typename CloudType>
69  NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags):
70  cloud(cloud),
71  dim(min(dim, int(cloud.rows()))),
72  creationOptionFlags(creationOptionFlags),
73  minBound(Vector::Constant(this->dim, numeric_limits<T>::max())),
74  maxBound(Vector::Constant(this->dim, numeric_limits<T>::min()))
75  {
76  if (cloud.cols() == 0)
77  throw runtime_error() << "Cloud has no points";
78  if (cloud.rows() == 0)
79  throw runtime_error() << "Cloud has 0 dimensions";
80  }
81 
82  template<typename T, typename CloudType>
83  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
84  {
85 #ifdef EIGEN3_API
86  const Eigen::Map<const Matrix> queryMatrix(&query.coeff(0,0), dim, 1);
87 #else // EIGEN3_API
88  const Eigen::Map<Matrix> queryMatrix(&query.coeff(0,0), dim, 1);
89 #endif // EIGEN3_API
90  // note: this is inefficient, because we copy memory, due to the template-
91  // based abstraction of Eigen. High-performance implementation should
92  // take care of knnM and then implement knn on top of it.
93  // C++0x should solve this with rvalue
94  IndexMatrix indexMatrix(k, 1);
95  Matrix dists2Matrix(k, 1);
96  const unsigned long stats = knn(queryMatrix, indexMatrix, dists2Matrix, k, epsilon, optionFlags, maxRadius);
97  indices = indexMatrix.col(0);
98  dists2 = dists2Matrix.col(0);
99  return stats;
100  }
101 
102  template<typename T, typename CloudType>
103  void NearestNeighbourSearch<T, CloudType>::checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k, const unsigned optionFlags, const Vector* maxRadii) const
104  {
105  const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T, CloudType>::ALLOW_SELF_MATCH);
106  if (allowSelfMatch)
107  {
108  if (k > cloud.cols())
109  throw runtime_error() << "Requesting more points (" << k << ") than available in cloud (" << cloud.cols() << ")";
110  }
111  else
112  {
113  if (k > cloud.cols()-1)
114  throw runtime_error() << "Requesting more points (" << k << ") than available in cloud minus 1 (" << cloud.cols()-1 << ") (as self match is forbidden)";
115  }
116  if (query.rows() < dim)
117  throw runtime_error() << "Query has less dimensions (" << query.rows() << ") than requested for cloud (" << dim << ")";
118  if (indices.rows() != k)
119  throw runtime_error() << "Index matrix has a different number of rows (" << indices.rows() << ") than k (" << k << ")";
120  if (indices.cols() != query.cols())
121  throw runtime_error() << "Index matrix has a different number of columns (" << indices.rows() << ") than query (" << query.cols() << ")";
122  if (dists2.rows() != k)
123  throw runtime_error() << "Distance matrix has a different number of rows (" << dists2.rows() << ") than k (" << k << ")";
124  if (dists2.cols() != query.cols())
125  throw runtime_error() << "Distance matrix has a different number of columns (" << dists2.rows() << ") than query (" << query.cols() << ")";
126  if (maxRadii && (maxRadii->size() != query.cols()))
127  throw runtime_error() << "Maximum radii vector has not the same length (" << maxRadii->size() << ") than query has columns (" << k << ")";
128  const unsigned maxOptionFlagsValue(ALLOW_SELF_MATCH|SORT_RESULTS);
129  if (optionFlags > maxOptionFlagsValue)
130  throw runtime_error() << "OR-ed value of option flags (" << optionFlags << ") is larger than maximal valid value (" << maxOptionFlagsValue << ")";
131  }
132 
133 
134  template<typename T, typename CloudType>
135  NearestNeighbourSearch<T, CloudType>* NearestNeighbourSearch<T, CloudType>::create(const CloudType& cloud, const Index dim, const SearchType preferedType, const unsigned creationOptionFlags, const Parameters& additionalParameters)
136  {
137  if (dim <= 0)
138  throw runtime_error() << "Your space must have at least one dimension";
139  switch (preferedType)
140  {
141  case BRUTE_FORCE: return new BruteForceSearch<T, CloudType>(cloud, dim, creationOptionFlags);
142  case KDTREE_LINEAR_HEAP: return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapBruteForceVector<Index,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
143  case KDTREE_TREE_HEAP: return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapSTL<Index,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
144  #ifdef HAVE_OPENCL
145  case KDTREE_CL_PT_IN_NODES: return new KDTreeBalancedPtInNodesStackOpenCL<T, CloudType>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
146  case KDTREE_CL_PT_IN_LEAVES: return new KDTreeBalancedPtInLeavesStackOpenCL<T, CloudType>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
147  case BRUTE_FORCE_CL: return new BruteForceSearchOpenCL<T, CloudType>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
148  #else // HAVE_OPENCL
149  case KDTREE_CL_PT_IN_NODES: throw runtime_error() << "OpenCL not found during compilation";
150  case KDTREE_CL_PT_IN_LEAVES: throw runtime_error() << "OpenCL not found during compilation";
151  case BRUTE_FORCE_CL: throw runtime_error() << "OpenCL not found during compilation";
152  #endif // HAVE_OPENCL
153  default: throw runtime_error() << "Unknown search type";
154  }
155  }
156 
157  template<typename T, typename CloudType>
159  {
160  if (dim <= 0)
161  throw runtime_error() << "Your space must have at least one dimension";
162  return new BruteForceSearch<T, CloudType>(cloud, dim, creationOptionFlags);
163  }
164 
165  template<typename T, typename CloudType>
166  NearestNeighbourSearch<T, CloudType>* NearestNeighbourSearch<T, CloudType>::createKDTreeLinearHeap(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters)
167  {
168  if (dim <= 0)
169  throw runtime_error() << "Your space must have at least one dimension";
170  return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapBruteForceVector<Index,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
171  }
172 
173  template<typename T, typename CloudType>
174  NearestNeighbourSearch<T, CloudType>* NearestNeighbourSearch<T, CloudType>::createKDTreeTreeHeap(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters)
175  {
176  if (dim <= 0)
177  throw runtime_error() << "Your space must have at least one dimension";
178  return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapSTL<Index,T>, CloudType>(cloud, dim, creationOptionFlags, additionalParameters);
179  }
180 
181  template struct NearestNeighbourSearch<float>;
182  template struct NearestNeighbourSearch<double>;
187 }
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
a column-major Eigen matrix in which each column is a point; this matrix has dim rows ...
Definition: nabo.h:263
KDTree, unbalanced, points in leaves, stack, implicit bounds, ANN_KD_SL_MIDPT, optimised implementati...
Definition: nabo_private.h:94
tuple knn(const object query, const Index k=1, const double epsilon=0, const unsigned optionFlags=0, const double maxRadius=infD)
implementation of index heaps
Nearest neighbour search interface, templatized on scalar type.
Definition: nabo.h:258
runtime_error(const runtime_error &re)
Definition: nabo/nabo.cpp:64
runtime_error & operator<<(const T &t)
Definition: nabo/nabo.cpp:53
Definition: any.hpp:450
NearestNeighbourSearch(const CloudType &cloud, const Index dim, const unsigned creationOptionFlags)
constructor
Definition: nabo/nabo.cpp:69
Eigen::Matrix< T, Eigen::Dynamic, 1 > Vector
an Eigen vector of type T, to hold the coordinates of a point
Definition: nabo.h:261
int Index
an index to a Vector or a Matrix, for refering to data points
Definition: nabo.h:267
NNSNabo::Matrix cloud
Eigen::Matrix< Index, Eigen::Dynamic, Eigen::Dynamic > IndexMatrix
a matrix of indices to data points
Definition: nabo.h:271
Brute-force nearest neighbour.
Definition: nabo_private.h:72
Parameter vector.
Definition: nabo.h:231
Eigen::Matrix< Index, Eigen::Dynamic, 1 > IndexVector
a vector of indices to data points
Definition: nabo.h:269
stringstream ss
Definition: nabo/nabo.cpp:51
virtual ~runtime_error()
Definition: nabo/nabo.cpp:65
Namespace for Nabo.
public interface
CloudType CloudType
a column-major Eigen matrix in which each column is a point; this matrix has dim rows ...
Definition: nabo.h:265
header for implementation


libnabo
Author(s): Stéphane Magnenat
autogenerated on Mon Feb 28 2022 22:41:38