brute_force_cpu.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_private.h"
00033 #include "index_heap.h"
00034 
00040 namespace Nabo
00041 {
00042         using namespace std;
00043         
00044         template<typename T, typename CloudType>
00045         BruteForceSearch<T, CloudType>::BruteForceSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags):
00046                 NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(cloud, dim, creationOptionFlags)
00047         {
00048 #ifdef EIGEN3_API
00049                 const_cast<Vector&>(this->minBound) = cloud.topRows(this->dim).rowwise().minCoeff();
00050                 const_cast<Vector&>(this->maxBound) = cloud.topRows(this->dim).rowwise().maxCoeff();
00051 #else // EIGEN3_API
00052                 // compute bounds
00053                 for (int i = 0; i < cloud.cols(); ++i)
00054                 {
00055                         const Vector& v(cloud.block(0,i,this->dim,1));
00056                         const_cast<Vector&>(this->minBound) = this->minBound.cwise().min(v);
00057                         const_cast<Vector&>(this->maxBound) = this->maxBound.cwise().max(v);
00058                 }
00059 #endif // EIGEN3_API
00060         }
00061         
00062 
00063         template<typename T, typename CloudType>
00064         unsigned long BruteForceSearch<T, CloudType>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
00065         {
00066                 const Vector maxRadii(Vector::Constant(query.cols(), maxRadius));
00067                 return knn(query, indices, dists2, maxRadii, k, epsilon, optionFlags);
00068         }
00069         
00070         template<typename T, typename CloudType>
00071         unsigned long BruteForceSearch<T, CloudType>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k, const T /*epsilon*/, const unsigned optionFlags) const
00072         {
00073                 checkSizesKnn(query, indices, dists2, k, optionFlags, &maxRadii);
00074                 
00075                 const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
00076                 const bool sortResults(optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS);
00077                 const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);
00078                 
00079                 IndexHeapSTL<Index, T> heap(k);
00080                 
00081                 for (int c = 0; c < query.cols(); ++c)
00082                 {
00083                         const T maxRadius(maxRadii[c]);
00084                         const T maxRadius2(maxRadius * maxRadius);
00085                         const Vector& q(query.block(0,c,dim,1));
00086                         heap.reset();
00087                         for (int i = 0; i < this->cloud.cols(); ++i)
00088                         {
00089                                 const T dist(dist2<T>(this->cloud.block(0,i,dim,1), q));
00090                                 if ((dist <= maxRadius2) &&
00091                                         (dist < heap.headValue()) &&
00092                                         (allowSelfMatch || (dist > numeric_limits<T>::epsilon())))
00093                                         heap.replaceHead(i, dist);
00094                         }
00095                         if (sortResults)
00096                                 heap.sort();    
00097                         heap.getData(indices.col(c), dists2.col(c));
00098                 }
00099                 if (collectStatistics)
00100                         return (unsigned long)query.cols() * (unsigned long)this->cloud.cols();
00101                 else
00102                         return 0;
00103         }
00104         
00105         template struct BruteForceSearch<float>;
00106         template struct BruteForceSearch<double>;
00107         template struct BruteForceSearch<float, Eigen::Matrix3Xf>;
00108         template struct BruteForceSearch<double, Eigen::Matrix3Xd>;
00109         template struct BruteForceSearch<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
00110         template struct BruteForceSearch<double, Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
00111 }


libnabo
Author(s): Stéphane Magnenat
autogenerated on Sun Feb 10 2019 03:52:20