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>
00050     NearestNeighbourSearch<T>::NearestNeighbourSearch(const Matrix& 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         
00058     }
00059     
00060     template<typename T>
00061     unsigned long NearestNeighbourSearch<T>::knn(const Vector& query, IndexVector& indices, Vector& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
00062     {
00063 #ifdef EIGEN3_API
00064         const Eigen::Map<const Matrix> queryMatrix(&query.coeff(0,0), dim, 1);
00065 #else // EIGEN3_API
00066         const Eigen::Map<Matrix> queryMatrix(&query.coeff(0,0), dim, 1);
00067 #endif // EIGEN3_API
00068         // note: this is inefficient, because we copy memory, due to the template-
00069         // based abstraction of Eigen. High-performance implementation should
00070         // take care of knnM and then implement knn on top of it.
00071         // C++0x should solve this with rvalue
00072         IndexMatrix indexMatrix(k, 1);
00073         Matrix dists2Matrix(k, 1);
00074         const unsigned long stats = knn(queryMatrix, indexMatrix, dists2Matrix, k, epsilon, optionFlags, maxRadius);
00075         indices = indexMatrix.col(0);
00076         dists2 = dists2Matrix.col(0);
00077         return stats;
00078     }
00079     
00080     template<typename T>
00081     void NearestNeighbourSearch<T>::checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k, const Vector* maxRadii) const
00082     {
00083         if (query.rows() < dim)
00084             throw runtime_error((boost::format("Query has less dimensions (%1%) than requested for cloud (%2%)") % query.rows() % dim).str());
00085         if (indices.rows() != k)
00086             throw runtime_error((boost::format("Index matrix has a different number of rows (%1%) than k (%2%)") % indices.rows() % k).str());
00087         if (indices.cols() != query.cols())
00088             throw runtime_error((boost::format("Index matrix has a different number of columns (%1%) than query (%2%)") % indices.rows() % query.cols()).str());
00089         if (dists2.rows() != k)
00090             throw runtime_error((boost::format("Distance matrix has a different number of rows (%1%) than k (%2%)") % dists2.rows() % k).str());
00091         if (dists2.cols() != query.cols())
00092             throw runtime_error((boost::format("Distance matrix has a different number of columns (%1%) than query (%2%)") % dists2.rows() % query.cols()).str());
00093         if (maxRadii && (maxRadii->size() != query.cols()))
00094             throw runtime_error((boost::format("Maximum radii vector has not the same length (%1%) than query has columns (%2%)") % maxRadii->size() % k).str());
00095     }
00096     
00097     
00098     template<typename T>
00099     NearestNeighbourSearch<T>* NearestNeighbourSearch<T>::create(const Matrix& cloud, const Index dim, const SearchType preferedType, const unsigned creationOptionFlags, const Parameters& additionalParameters)
00100     {
00101         if (dim <= 0)
00102             throw runtime_error("Your space must have at least one dimension");
00103         switch (preferedType)
00104         {
00105             case BRUTE_FORCE: return new BruteForceSearch<T>(cloud, dim, creationOptionFlags);
00106             case KDTREE_LINEAR_HEAP: return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapBruteForceVector<int,T> >(cloud, dim, creationOptionFlags, additionalParameters);
00107             case KDTREE_TREE_HEAP: return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapSTL<int,T> >(cloud, dim, creationOptionFlags, additionalParameters);
00108             #ifdef HAVE_OPENCL
00109             case KDTREE_CL_PT_IN_NODES: return new KDTreeBalancedPtInNodesStackOpenCL<T>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
00110             case KDTREE_CL_PT_IN_LEAVES: return new KDTreeBalancedPtInLeavesStackOpenCL<T>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
00111             case BRUTE_FORCE_CL: return new BruteForceSearchOpenCL<T>(cloud, dim, creationOptionFlags, CL_DEVICE_TYPE_GPU);
00112             #else // HAVE_OPENCL
00113             case KDTREE_CL_PT_IN_NODES: throw runtime_error("OpenCL not found during compilation");
00114             case KDTREE_CL_PT_IN_LEAVES: throw runtime_error("OpenCL not found during compilation");
00115             case BRUTE_FORCE_CL: throw runtime_error("OpenCL not found during compilation");
00116             #endif // HAVE_OPENCL
00117             default: throw runtime_error("Unknown search type");
00118         }
00119     }
00120     
00121     template<typename T>
00122     NearestNeighbourSearch<T>* NearestNeighbourSearch<T>::createBruteForce(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags)
00123     {
00124         if (dim <= 0)
00125             throw runtime_error("Your space must have at least one dimension");
00126         return new BruteForceSearch<T>(cloud, dim, creationOptionFlags);
00127     }
00128     
00129     template<typename T>
00130     NearestNeighbourSearch<T>* NearestNeighbourSearch<T>::createKDTreeLinearHeap(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters)
00131     {
00132         if (dim <= 0)
00133             throw runtime_error("Your space must have at least one dimension");
00134         return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapBruteForceVector<int,T> >(cloud, dim, creationOptionFlags, additionalParameters);
00135     }
00136     
00137     template<typename T>
00138     NearestNeighbourSearch<T>* NearestNeighbourSearch<T>::createKDTreeTreeHeap(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters)
00139     {
00140         if (dim <= 0)
00141             throw runtime_error("Your space must have at least one dimension");
00142         return new KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, IndexHeapSTL<int,T> >(cloud, dim, creationOptionFlags, additionalParameters);
00143     }
00144     
00145     template struct NearestNeighbourSearch<float>;
00146     template struct NearestNeighbourSearch<double>;
00147 }


libnabo
Author(s): Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:27:28