nabo.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
00004 You can contact the author at <stephane at magnenat dot net>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010     * Redistributions of source code must retain the above copyright
00011       notice, this list of conditions and the following disclaimer.
00012     * Redistributions in binary form must reproduce the above copyright
00013       notice, this list of conditions and the following disclaimer in the
00014       documentation and/or other materials provided with the distribution.
00015     * Neither the name of the <organization> nor the
00016       names of its contributors may be used to endorse or promote products
00017       derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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                 // note: this is inefficient, because we copy memory, due to the template-
00072                 // based abstraction of Eigen. High-performance implementation should
00073                 // take care of knnM and then implement knn on top of it.
00074                 // C++0x should solve this with rvalue
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 }


libnabo
Author(s): Stéphane Magnenat
autogenerated on Thu Sep 10 2015 10:54:55