From bc57fc36b48bcc40da02b3d308c1386ff0d1f5b2 Mon Sep 17 00:00:00 2001 From: cignoni Date: Wed, 22 Feb 2012 16:57:44 +0000 Subject: [PATCH] Moved here and cleaned the kdtree for points implemented by Gael --- vcg/space/index/kdtree/kdtree.h | 336 +++++++++++++++++++++++++ vcg/space/index/kdtree/mlsutils.h | 119 +++++++++ vcg/space/index/kdtree/priorityqueue.h | 122 +++++++++ 3 files changed, 577 insertions(+) create mode 100755 vcg/space/index/kdtree/kdtree.h create mode 100755 vcg/space/index/kdtree/mlsutils.h create mode 100755 vcg/space/index/kdtree/priorityqueue.h diff --git a/vcg/space/index/kdtree/kdtree.h b/vcg/space/index/kdtree/kdtree.h new file mode 100755 index 00000000..e3dc70b5 --- /dev/null +++ b/vcg/space/index/kdtree/kdtree.h @@ -0,0 +1,336 @@ +#ifndef KDTREE_H +#define KDTREE_H + +#include "../../point3.h" +#include "../../box3.h" +#include "mlsutils.h" +#include "priorityqueue.h" +#include +#include +#include + + + +template +class ConstDataWrapper +{ +public: + typedef _DataType DataType; + inline ConstDataWrapper() + : mpData(0), mStride(0), mSize(0) + {} + inline ConstDataWrapper(const DataType* pData, int size, int stride = sizeof(DataType)) + : mpData(reinterpret_cast(pData)), mStride(stride), mSize(size) + {} + inline const DataType& operator[] (int i) const + { + return *reinterpret_cast(mpData + i*mStride); + } + inline size_t size() const { return mSize; } +protected: + const unsigned char* mpData; + int mStride; + size_t mSize; +}; + +/** + * This class allows to create a Kd-Tree thought to perform the k-nearest neighbour query + */ +template +class KdTree +{ +public: + + typedef _Scalar Scalar; + typedef vcg::Point3 VectorType; + typedef vcg::Box3 AxisAlignedBoxType; + + struct Node + { + union { + //standard node + struct { + Scalar splitValue; + unsigned int firstChildId:24; + unsigned int dim:2; + unsigned int leaf:1; + }; + //leaf + struct { + unsigned int start; + unsigned short size; + }; + }; + }; + typedef std::vector NodeList; + + // return the protected members which store the nodes and the points list + inline const NodeList& _getNodes(void) { return mNodes; } + inline const std::vector& _getPoints(void) { return mPoints; } + + + void setMaxNofNeighbors(unsigned int k); + inline int getNofFoundNeighbors(void) { return mNeighborQueue.getNofElements(); } + inline const VectorType& getNeighbor(int i) { return mPoints[ mNeighborQueue.getIndex(i) ]; } + inline unsigned int getNeighborId(int i) { return mIndices[mNeighborQueue.getIndex(i)]; } + inline float getNeighborSquaredDistance(int i) { return mNeighborQueue.getWeight(i); } + +public: + + KdTree(const ConstDataWrapper& points, unsigned int nofPointsPerCell = 16, unsigned int maxDepth = 64); + + ~KdTree(); + + void doQueryK(const VectorType& p); + +protected: + + // element of the stack + struct QueryNode + { + QueryNode() {} + QueryNode(unsigned int id) : nodeId(id) {} + unsigned int nodeId; // id of the next node + Scalar sq; // squared distance to the next node + }; + + // used to build the tree: split the subset [start..end[ according to dim and splitValue, + // and returns the index of the first element of the second subset + unsigned int split(int start, int end, unsigned int dim, float splitValue); + + void createTree(unsigned int nodeId, unsigned int start, unsigned int end, unsigned int level, unsigned int targetCellsize, unsigned int targetMaxDepth); + +protected: + + AxisAlignedBoxType mAABB; //BoundingBox + NodeList mNodes; //kd-tree nodes + std::vector mPoints; //points read from the input DataWrapper + std::vector mIndices; //points indices + + HeapMaxPriorityQueue mNeighborQueue; //used to perform the knn-query + QueryNode mNodeStack[64]; //used in the implementation of the knn-query +}; + +template +KdTree::KdTree(const ConstDataWrapper& points, unsigned int nofPointsPerCell, unsigned int maxDepth) + : mPoints(points.size()), mIndices(points.size()) +{ + // compute the AABB of the input + mPoints[0] = points[0]; + mAABB.Set(mPoints[0]); + for (unsigned int i=1 ; i +KdTree::~KdTree() +{ +} + +template +void KdTree::setMaxNofNeighbors(unsigned int k) +{ + mNeighborQueue.setMaxSize(k); +} + +/** Performs the kNN query. + * + * This algorithm uses the simple distance to the split plane to prune nodes. + * A more elaborated approach consists to track the closest corner of the cell + * relatively to the current query point. This strategy allows to save about 5% + * of the leaves. However, in practice the slight overhead due to this tracking + * reduces the overall performance. + * + * This algorithm also use a simple stack while a priority queue using the squared + * distances to the cells as a priority values allows to save about 10% of the leaves. + * But, again, priority queue insertions and deletions are quite involved, and therefore + * a simple stack is by far much faster. + * + * The result of the query, the k-nearest neighbors, are internally stored into a stack, where the + * topmost element + */ +template +void KdTree::doQueryK(const VectorType& queryPoint) +{ + mNeighborQueue.init(); + mNeighborQueue.insert(0xffffffff, std::numeric_limits::max()); + + mNodeStack[0].nodeId = 0; + mNodeStack[0].sq = 0.f; + unsigned int count = 1; + + while (count) + { + //we select the last node (AABB) inserted in the stack + QueryNode& qnode = mNodeStack[count-1]; + + //while going down the tree qnode.nodeId is the nearest sub-tree, otherwise, + //in backtracking, qnode.nodeId is the other sub-tree that will be visited iff + //the actual nearest node is further than the split distance. + Node& node = mNodes[qnode.nodeId]; + + //if the distance is less than the top of the max-heap, it could be one of the k-nearest neighbours + if (qnode.sq < mNeighborQueue.getTopWeight()) + { + //when we arrive to a lef + if (node.leaf) + { + --count; //pop of the leaf + + //end is the index of the last element of the leaf in mPoints + unsigned int end = node.start+node.size; + //adding the element of the leaf to the heap + for (unsigned int i=node.start ; i +unsigned int KdTree::split(int start, int end, unsigned int dim, float splitValue) +{ + int l(start), r(end-1); + for ( ; l= start && mPoints[r][dim] >= splitValue) + r--; + if (l > r) + break; + std::swap(mPoints[l],mPoints[r]); + std::swap(mIndices[l],mIndices[r]); + } + //returns the index of the first element on the second part + return (mPoints[l][dim] < splitValue ? l+1 : l); +} + +/** recursively builds the kdtree + * + * The heuristic is the following: + * - if the number of points in the node is lower than targetCellsize then make a leaf + * - else compute the AABB of the points of the node and split it at the middle of + * the largest AABB dimension. + * + * This strategy might look not optimal because it does not explicitly prune empty space, + * unlike more advanced SAH-like techniques used for RT. On the other hand it leads to a shorter tree, + * faster to traverse and our experience shown that in the special case of kNN queries, + * this strategy is indeed more efficient (and much faster to build). Moreover, for volume data + * (e.g., fluid simulation) pruning the empty space is useless. + * + * Actually, storing at each node the exact AABB (we therefore have a binary BVH) allows + * to prune only about 10% of the leaves, but the overhead of this pruning (ball/ABBB intersection) + * is more expensive than the gain it provides and the memory consumption is x4 higher ! + */ +template +void KdTree::createTree(unsigned int nodeId, unsigned int start, unsigned int end, unsigned int level, unsigned int targetCellSize, unsigned int targetMaxDepth) +{ + //select the first node + Node& node = mNodes[nodeId]; + AxisAlignedBoxType aabb; + + //putting all the points in the bounding box + aabb.Set(mPoints[start]); + for (unsigned int i=start+1 ; i=targetMaxDepth) + { + child.leaf = 1; + child.start = start; + child.size = midId - start; + } + else + { + child.leaf = 0; + createTree(childId, start, midId, level+1, targetCellSize, targetMaxDepth); + } + } + + { + // right child + unsigned int childId = mNodes[nodeId].firstChildId+1; + Node& child = mNodes[childId]; + if (end - midId <= targetCellSize || level>=targetMaxDepth) + { + child.leaf = 1; + child.start = midId; + child.size = end - midId; + } + else + { + child.leaf = 0; + createTree(childId, midId, end, level+1, targetCellSize, targetMaxDepth); + } + } +} + +#endif + diff --git a/vcg/space/index/kdtree/mlsutils.h b/vcg/space/index/kdtree/mlsutils.h new file mode 100755 index 00000000..44a2ef47 --- /dev/null +++ b/vcg/space/index/kdtree/mlsutils.h @@ -0,0 +1,119 @@ +/**************************************************************************** +* MeshLab o o * +* A versatile mesh processing toolbox o o * +* _ O _ * +* Copyright(C) 2005 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ + +#ifndef VCGADDONS_H +#define VCGADDONS_H + + +namespace vcg { + +//template +//inline Point3 CwiseMul(Point3 const & p1, Point3 const & p2) +//{ +// return Point3(p1.X()*p2.X(), p1.Y()*p2.Y(), p1.Z()*p2.Z()); +//} + +//template +//inline Point3 Min(Point3 const & p1, Point3 const & p2) +//{ +// return Point3(std::min(p1.X(), p2.X()), std::min(p1.Y(), p2.Y()), std::min(p1.Z(), p2.Z())); +//} + +//template +//inline Point3 Max(Point3 const & p1, Point3 const & p2) +//{ +// return Point3(std::max(p1.X(), p2.X()), std::max(p1.Y(), p2.Y()), std::max(p1.Z(), p2.Z())); +//} + +template +inline Scalar MaxCoeff(Point3 const & p) +{ + return std::max(std::max(p.X(), p.Y()), p.Z()); +} + +//template +//inline Scalar MinCoeff(Point3 const & p) +//{ +// return std::min(std::min(p.X(), p.Y()), p.Z()); +//} + +template +inline Scalar Dot(Point3 const & p1, Point3 const & p2) +{ + return p1.X() * p2.X() + p1.Y() * p2.Y() + p1.Z() * p2.Z(); +} + +//template +//inline Point3 Cross(Point3 const & p1, Point3 const & p2) +//{ +// return p1 ^ p2; +//} + +//template +//inline Point3 CwiseAdd(Point3 const & p1, Scalar s) +//{ +// return Point3(p1.X() + s, p1.Y() + s, p1.Z() + s); +//} + +template +inline int MaxCoeffId(Point3 const & p) +{ + if (p.X()>p.Y()) + return p.X()>p.Z() ? 0 : 2; + else + return p.Y()>p.Z() ? 1 : 2; +} + +//template +//inline int MinCoeffId(Point3 const & p) +//{ +// if (p.X() +//inline Point3 Point3Cast(const Point3& p) +//{ +// return Point3(p.X(), p.Y(), p.Z()); +//} + +//template +//Scalar Distance(const Point3 &p, const Box3 &bbox) +//{ +// Scalar dist2 = 0.; +// Scalar aux; +// for (int k=0 ; k<3 ; ++k) +// { +// if ( (aux = (p[k]-bbox.min[k]))<0. ) +// dist2 += aux*aux; +// else if ( (aux = (bbox.max[k]-p[k]))<0. ) +// dist2 += aux*aux; +// } +// return sqrt(dist2); +//} + +} + +#endif diff --git a/vcg/space/index/kdtree/priorityqueue.h b/vcg/space/index/kdtree/priorityqueue.h new file mode 100755 index 00000000..13aae024 --- /dev/null +++ b/vcg/space/index/kdtree/priorityqueue.h @@ -0,0 +1,122 @@ +/**************************************************************************** +* MeshLab o o * +* A versatile mesh processing toolbox o o * +* _ O _ * +* Copyright(C) 2005 \/)\/ * +* Visual Computing Lab /\/| * +* ISTI - Italian National Research Council | * +* \ * +* All rights reserved. * +* * +* This program is free software; you can redistribute it and/or modify * +* it under the terms of the GNU General Public License as published by * +* the Free Software Foundation; either version 2 of the License, or * +* (at your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * +* for more details. * +* * +****************************************************************************/ + +#ifndef _PriorityQueue_h_ +#define _PriorityQueue_h_ + +/** Implements a bounded-size max priority queue using a heap +*/ +template +class HeapMaxPriorityQueue +{ + struct Element + { + Weight weight; + Index index; + }; + +public: + + HeapMaxPriorityQueue(void) + { + mElements = 0; + mMaxSize = 0; + } + + inline void setMaxSize(int maxSize) + { + if (mMaxSize!=maxSize) + { + mMaxSize = maxSize; + delete[] mElements; + mElements = new Element[mMaxSize]; + mpOffsetedElements = (mElements-1); + } + init(); + } + + inline void init() { mCount = 0; } + + inline bool isFull() const { return mCount == mMaxSize; } + + /** returns number of elements inserted in queue + */ + inline int getNofElements() const { return mCount; } + + inline Weight getWeight(int i) const { return mElements[i].weight; } + inline Index getIndex(int i) const { return mElements[i].index; } + + inline Weight getTopWeight() const { return mElements[0].weight; } + + inline void insert(Index index, Weight weight) + { + if (mCount==mMaxSize) + { + if (weightweight < mpOffsetedElements[k+1].weight)) + z = &(mpOffsetedElements[++k]); + + if(weight >= z->weight) + break; + mpOffsetedElements[j] = *z; + j = k; + k = 2 * j; + } + mpOffsetedElements[j].weight = weight; + mpOffsetedElements[j].index = index; + } + } + else + { + int i, j; + i = ++mCount; + while (i >= 2) + { + j = i >> 1; + Element& y = mpOffsetedElements[j]; + if(weight <= y.weight) + break; + mpOffsetedElements[i] = y; + i = j; + } + mpOffsetedElements[i].index = index; + mpOffsetedElements[i].weight = weight; + } + } + +protected: + + int mCount; + int mMaxSize; + Element* mElements; + Element* mpOffsetedElements; +}; + +#endif