nabo_private.h

Go to the documentation of this file.

/*
 
Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the author at <stephane at magnenat dot net>
 
All rights reserved.
 
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.
 
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
*/
 
#ifndef __NABO_PRIVATE_H
#define __NABO_PRIVATE_H
 
#include "nabo.h"
 
#include <cstdint>
using std::uint32_t;
 
// OpenCL
#ifdef HAVE_OPENCL
        #define __CL_ENABLE_EXCEPTIONS
        #include "CL/cl.hpp"
#endif // HAVE_OPENCL
 
// Unused macro, add support for your favorite compiler
#if defined(__GNUC__)
        #define _UNUSED __attribute__ ((unused))
#else
        #define _UNUSED
#endif
 
namespace Nabo
{
 
        
        template<typename T, typename A, typename B>
        inline T dist2(const A& v0, const B& v1)
        {
                return (v0 - v1).squaredNorm();
        }
 
        template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
        struct BruteForceSearch : public NearestNeighbourSearch<T, CloudType>
        {
                typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
                typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
                typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
                
                using NearestNeighbourSearch<T, CloudType>::dim;
                using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
                using NearestNeighbourSearch<T, CloudType>::checkSizesKnn;
                using NearestNeighbourSearch<T, CloudType>::minBound;
                using NearestNeighbourSearch<T, CloudType>::maxBound;
 
                BruteForceSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags);
                virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
                virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
        };
        
        template<typename T, typename Heap, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
        struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt: public NearestNeighbourSearch<T, CloudType>
        {
                typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
                typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
                typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
                
                using NearestNeighbourSearch<T, CloudType>::dim;
                using NearestNeighbourSearch<T, CloudType>::cloud;
                using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
                using NearestNeighbourSearch<T, CloudType>::minBound;
                using NearestNeighbourSearch<T, CloudType>::maxBound;
                using NearestNeighbourSearch<T, CloudType>::checkSizesKnn;
                
        protected:
                typedef std::vector<Index> BuildPoints;
                typedef typename BuildPoints::iterator BuildPointsIt;
                typedef typename BuildPoints::const_iterator BuildPointsCstIt;
                
                const unsigned bucketSize;
                
                const uint32_t dimBitCount;
                const uint32_t dimMask;
                
                inline uint32_t createDimChildBucketSize(const uint32_t dim, const uint32_t childIndex) const
                { return dim | (childIndex << dimBitCount); }
                inline uint32_t getDim(const uint32_t dimChildBucketSize) const
                { return dimChildBucketSize & dimMask; }
                inline uint32_t getChildBucketSize(const uint32_t dimChildBucketSize) const
                { return dimChildBucketSize >> dimBitCount; }
                
                struct BucketEntry;
                
                struct Node
                {
                        uint32_t dimChildBucketSize; 
                        union
                        {
                                T cutVal; 
                                uint32_t bucketIndex; 
                        };
                        
                        Node(const uint32_t dimChild, const T cutVal):
                                dimChildBucketSize(dimChild), cutVal(cutVal) {}
                        Node(const uint32_t bucketSize, const uint32_t bucketIndex):
                                dimChildBucketSize(bucketSize), bucketIndex(bucketIndex) {}
                };
                typedef std::vector<Node> Nodes;
                
                struct BucketEntry
                {
                        const T* pt; 
                        Index index; 
                        
 
                        BucketEntry(const T* pt = 0, const Index index = 0): pt(pt), index(index) {}
                };
                
                typedef std::vector<BucketEntry> Buckets;
                
                Nodes nodes;
                
                Buckets buckets;
                
                std::pair<T,T> getBounds(const BuildPointsIt first, const BuildPointsIt last, const unsigned dim);
                unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues);
                
 
                unsigned long onePointKnn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, int i, Heap& heap, std::vector<T>& off, const T maxError, const T maxRadius2, const bool allowSelfMatch, const bool collectStatistics, const bool sortResults) const;
                
 
                template<bool allowSelfMatch, bool collectStatistics>
                unsigned long recurseKnn(const T* query, const unsigned n, T rd, Heap& heap, std::vector<T>& off, const T maxError, const T maxRadius2) const;
                
        public:
                KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters);
                virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
                virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
        };
        
        #ifdef HAVE_OPENCL
        
        template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
        struct OpenCLSearch: public NearestNeighbourSearch<T, CloudType>
        {
                typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
                typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
                typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
                
                using NearestNeighbourSearch<T, CloudType>::dim;
                using NearestNeighbourSearch<T, CloudType>::cloud;
                using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
                using NearestNeighbourSearch<T, CloudType>::checkSizesKnn;
                
        protected:
                const cl_device_type deviceType; 
                cl::Context& context; 
                mutable cl::Kernel knnKernel; 
                cl::CommandQueue queue; 
                cl::Buffer cloudCL; 
                
                OpenCLSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
 
                void initOpenCL(const char* clFileName, const char* kernelName, const std::string& additionalDefines = "");
        
        public:
                virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
                virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
        };
        
        template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
        struct BruteForceSearchOpenCL: public OpenCLSearch<T, CloudType>
        {
                typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
                typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
                typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
                
                using OpenCLSearch<T, CloudType>::initOpenCL;
                
                BruteForceSearchOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
        };
        
        template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
        struct KDTreeBalancedPtInLeavesStackOpenCL: public OpenCLSearch<T, CloudType>
        {
                typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
                typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
                typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
                
                using NearestNeighbourSearch<T, CloudType>::dim;
                using NearestNeighbourSearch<T, CloudType>::cloud;
                using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
                using NearestNeighbourSearch<T, CloudType>::minBound;
                using NearestNeighbourSearch<T, CloudType>::maxBound;
                
                using OpenCLSearch<T, CloudType>::context;
                using OpenCLSearch<T, CloudType>::knnKernel;
                
                using OpenCLSearch<T, CloudType>::initOpenCL;
                
        protected:
                struct BuildPoint
                {
                        Vector pos; 
                        size_t index; 
                        BuildPoint(const Vector& pos =  Vector(), const size_t index = 0): pos(pos), index(index) {}
                };
                typedef std::vector<BuildPoint> BuildPoints;
                typedef typename BuildPoints::iterator BuildPointsIt;
                typedef typename BuildPoints::const_iterator BuildPointsCstIt;
                
                struct CompareDim
                {
                        size_t dim; 
                        CompareDim(const size_t dim):dim(dim){}
                        bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
                };
                
                struct Node
                {
                        int dim; 
                        T cutVal; 
                        Node(const int dim = -1, const T cutVal = 0):dim(dim), cutVal(cutVal) {}
                };
                typedef std::vector<Node> Nodes;
                
                Nodes nodes; 
                cl::Buffer nodesCL; 
                
                
                inline size_t childLeft(size_t pos) const { return 2*pos + 1; } 
                inline size_t childRight(size_t pos) const { return 2*pos + 2; } 
                inline size_t parent(size_t pos) const { return (pos-1)/2; } 
                size_t getTreeDepth(size_t size) const; 
                size_t getTreeSize(size_t size) const; 
                
                void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
                
        public:
                KDTreeBalancedPtInLeavesStackOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
        };
        
        template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
        struct KDTreeBalancedPtInNodesStackOpenCL: public OpenCLSearch<T, CloudType>
        {
                typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
                typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
                typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
                typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
                
                using NearestNeighbourSearch<T, CloudType>::dim;
                using NearestNeighbourSearch<T, CloudType>::cloud;
                using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
                using NearestNeighbourSearch<T, CloudType>::minBound;
                using NearestNeighbourSearch<T, CloudType>::maxBound;
                
                using OpenCLSearch<T, CloudType>::context;
                using OpenCLSearch<T, CloudType>::knnKernel;
                
                using OpenCLSearch<T, CloudType>::initOpenCL;
                
        protected:
                typedef Index BuildPoint;
                typedef std::vector<BuildPoint> BuildPoints;
                typedef typename BuildPoints::iterator BuildPointsIt;
                typedef typename BuildPoints::const_iterator BuildPointsCstIt;
                
                struct CompareDim
                {
                        const CloudType& cloud; 
                        size_t dim; 
                        CompareDim(const CloudType& cloud, const size_t dim): cloud(cloud), dim(dim){}
                        bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return cloud.coeff(dim, p0) < cloud.coeff(dim, p1); }
                };
                
                struct Node
                {
                        int dim; 
                        Index index; 
                        Node(const int dim = -2, const Index index = 0):dim(dim), index(index) {}
                };
                typedef std::vector<Node> Nodes;
                
                Nodes nodes; 
                cl::Buffer nodesCL;  
                
                
                inline size_t childLeft(size_t pos) const { return 2*pos + 1; } 
                inline size_t childRight(size_t pos) const { return 2*pos + 2; } 
                inline size_t parent(size_t pos) const { return (pos-1)/2; } 
                size_t getTreeDepth(size_t size) const; 
                size_t getTreeSize(size_t size) const; 
                
                void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
                
        public:
                KDTreeBalancedPtInNodesStackOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
        };
        
        #endif // HAVE_OPENCL
        
}
 
#endif // __NABO_H