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>
00045     BruteForceSearch<T>::BruteForceSearch(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags):
00046         NearestNeighbourSearch<T>::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>
00064     unsigned long BruteForceSearch<T>::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>
00071     unsigned long BruteForceSearch<T>::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, &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 }


libnabo
Author(s): St├ęphane Magnenat
autogenerated on Thu Jan 2 2014 11:15:54