00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #ifndef ApproxMVBB_KdTree_hpp
00011 #define ApproxMVBB_KdTree_hpp
00012
00013 #include <type_traits>
00014 #include <initializer_list>
00015 #include <memory>
00016 #include <algorithm>
00017 #include <array>
00018 #include <queue>
00019 #include <deque>
00020 #include <list>
00021 #include <tuple>
00022 #include <unordered_set>
00023 #include <unordered_map>
00024 #include <utility>
00025
00026 #include <fstream>
00027
00028 #include <meta/meta.hpp>
00029
00030 #include "ApproxMVBB/Config/Config.hpp"
00031
00032 #include ApproxMVBB_TypeDefs_INCLUDE_FILE
00033 #include ApproxMVBB_AssertionDebug_INCLUDE_FILE
00034
00035 #include "ApproxMVBB/Common/StaticAssert.hpp"
00036 #include "ApproxMVBB/Common/SfinaeMacros.hpp"
00037 #include "ApproxMVBB/Common/ContainerTag.hpp"
00038
00039 #include "ApproxMVBB/AABB.hpp"
00040
00041
00042
00043 namespace ApproxMVBB{
00044
00045 namespace KdTree {
00046
00047
00048 ApproxMVBB_DEFINE_MATRIX_TYPES
00049 ApproxMVBB_DEFINE_POINTS_CONFIG_TYPES
00050
00051
00052 namespace details{
00053 struct TakeDefault;
00054
00055 template<typename T>
00056 using isDefault = meta::or_<
00057 meta::_t<std::is_same<T,TakeDefault> >,
00058 meta::_t<std::is_same<T,void> >
00059 >;
00060 }
00061
00062 #define DEFINE_KDTREE_BASETYPES( __Traits__ ) \
00063 \
00064 using NodeDataType = typename __Traits__::NodeDataType; \
00065 static const unsigned int Dimension = __Traits__::NodeDataType::Dimension; \
00066 using NodeType = typename __Traits__::NodeType; \
00067 \
00068 using NodeContainerType = std::vector<NodeType *>; \
00069 using LeafContainerType = NodeContainerType; \
00070 using LeafNeighbourMapType = std::unordered_map<std::size_t, std::unordered_set<std::size_t> >;
00071
00072
00073 struct EuclideanDistSq {
00074 template <typename Derived>
00075 static
00076 typename Derived::Scalar apply( const MatrixBase<Derived> & p) {
00077 return p.squaredNorm();
00078 }
00079 };
00080
00081 template<typename TPoint, typename TPointGetter, typename DistSq = EuclideanDistSq>
00082 struct DistanceComp {
00083 DistanceComp() {
00084 m_ref.setZero();
00085 }
00086 template<typename T>
00087 DistanceComp( const T & ref ): m_ref(ref) {}
00088
00089 template<typename T>
00090 inline bool operator()(const T & p1, const T & p2) {
00091 return DistSq::apply(m_ref - TPointGetter::get(p1)) <
00092 DistSq::apply(m_ref - TPointGetter::get(p2));
00093 }
00094
00095 template<typename T>
00096 inline PREC operator()(const T & p1) {
00097 return DistSq::apply(m_ref - TPointGetter::get(p1));
00098 }
00099
00100 TPoint m_ref;
00101 };
00102
00104 template<typename TPoint, typename TPointGetter>
00105 struct DefaultDistanceCompTraits{
00106 template<typename TDistSq>
00107 using DistanceComp = DistanceComp<TPoint,TPointGetter>;
00108 };
00109
00111 template<unsigned int Dim = 3,
00112 typename TPoint = Vector3,
00113 typename TValue = Vector3 *,
00114 typename TPointGetter = details::TakeDefault,
00115 typename TDistanceCompTraits = details::TakeDefault
00116 >
00117 class DefaultPointDataTraits {
00118 public:
00119 static const unsigned int Dimension = Dim;
00120 using PointType = TPoint;
00121 using PointListType = StdVecAligned<TValue>;
00122
00123 using iterator = typename PointListType::iterator;
00124 using const_iterator= typename PointListType::const_iterator;
00125
00126
00127 private:
00128
00129 template<typename T>
00130 struct PointGetterImpl {
00131 inline static PointType & get(T & t) {
00132 return t;
00133 }
00134 inline static const PointType & get(const T & t) {
00135 return t;
00136 }
00137 };
00138
00139 template<typename TT>
00140 struct PointGetterImpl<TT*> {
00141 inline static PointType & get(TT * t) {
00142 return *t;
00143 }
00144 inline static const PointType & get(const TT * t) {
00145 return *t;
00146 }
00147 };
00148
00149
00150
00151 public:
00153 using PointGetter = meta::if_< details::isDefault<TPointGetter> ,
00154 PointGetterImpl<typename PointListType::value_type>,
00155 TPointGetter
00156 >;
00157
00159 using DistCompTraits = meta::if_< details::isDefault<TDistanceCompTraits> ,
00160 DefaultDistanceCompTraits<PointType,PointGetter>,
00161 TDistanceCompTraits
00162 >;
00163
00164 };
00165
00167 template<typename TTraits = DefaultPointDataTraits<> >
00168 class PointData {
00169 public:
00170
00171 using Traits = TTraits;
00172 static const unsigned int Dimension = Traits::Dimension;
00173 using PointType = typename Traits::PointType;
00174 using PointListType = typename Traits::PointListType;
00175 using iterator = typename Traits::iterator;
00176 using const_iterator= typename Traits::const_iterator;
00177 using PointGetter = typename Traits::PointGetter;
00178
00179 template<typename TDistSq>
00180 using DistanceComp = typename Traits::DistCompTraits::template DistanceComp<TDistSq>;
00181
00182
00183
00184
00189 PointData(iterator begin, iterator end, std::unique_ptr<PointListType> points = nullptr)
00190 : m_begin(begin), m_end(end), m_points(points.release()) {}
00191
00192 ~PointData() {
00193 if(m_points) {
00194 delete m_points;
00195 }
00196 }
00198 std::pair<PointData *, PointData * >
00199 split(iterator splitRightIt) const {
00200
00201 PointData * left = new PointData(m_begin, splitRightIt );
00202
00203 PointData * right = new PointData(splitRightIt, m_end);
00204 return std::make_pair(left,right);
00205
00206 }
00207
00208 PREC getGeometricMean(unsigned int axis) {
00209 PREC ret = 0.0;
00210
00211 for(auto & pPoint : *this) {
00212 ret += PointGetter::get(pPoint)(axis);
00213 }
00214 ret /= size();
00215 return ret;
00216 }
00217
00218 iterator begin() {
00219 return m_begin;
00220 }
00221
00222 iterator end() {
00223 return m_end;
00224 }
00225
00226
00227 const_iterator begin() const {
00228 return m_begin;
00229 }
00230
00231 const_iterator end() const {
00232 return m_end;
00233 }
00234
00235
00236 std::size_t size() const {
00237 return std::distance(m_begin,m_end);
00238 }
00239
00240 std::string getPointString() {
00241 std::stringstream ss;
00242 for(auto & pPoint : *this) {
00243 ss << PointGetter::get(pPoint).transpose().format(MyMatrixIOFormat::SpaceSep) << std::endl;
00244 }
00245 return ss.str();
00246 }
00247
00248
00249 friend class XML;
00250 private:
00251 iterator m_begin,m_end;
00252
00254 PointListType* m_points = nullptr;
00255 };
00256
00257
00266 class LinearQualityEvaluator {
00267 public:
00272 LinearQualityEvaluator(PREC ws = 0.0, PREC wp = 0.0, PREC we = 1.0):
00273 m_weightSplitRatio(ws), m_weightPointRatio(wp), m_weightMinMaxExtentRatio(we) {}
00274
00275 inline void setLevel(unsigned int level) {
00276
00277 }
00278
00279 inline PREC compute(const PREC & splitRatio, const PREC & pointRatio, const PREC & minMaxExtentRatio) {
00280 return m_weightSplitRatio * 2.0 * splitRatio
00281 + m_weightPointRatio * 2.0 * pointRatio
00282 + m_weightMinMaxExtentRatio * minMaxExtentRatio;
00283 }
00284
00285 private:
00287 PREC m_weightSplitRatio = 0.0;
00288 PREC m_weightPointRatio = 0.0;
00289 PREC m_weightMinMaxExtentRatio = 1.0;
00290 };
00291
00292
00293 template< typename Traits >
00294 class SplitHeuristic;
00295
00296 template< typename TQualityEvaluator, typename Traits >
00297 class SplitHeuristicPointData {
00298 public:
00299
00300 DEFINE_KDTREE_BASETYPES( Traits )
00301
00302 using PointDataType = NodeDataType;
00303 using PointListType = typename NodeDataType::PointListType;
00304 using PointType = typename PointDataType::PointType ;
00305 using iterator = typename PointDataType::iterator;
00306 using PointGetter = typename PointDataType::PointGetter;
00307
00308 enum class Method : char { MIDPOINT, MEDIAN, GEOMETRIC_MEAN };
00309
00314 enum class SearchCriteria : char { FIND_FIRST, FIND_BEST };
00315
00316 using QualityEvaluator = TQualityEvaluator;
00317
00318 using SplitAxisType = typename NodeType::SplitAxisType;
00319
00320
00321 SplitHeuristicPointData() : m_methods({Method::MIDPOINT}), m_searchCriteria(SearchCriteria::FIND_BEST) {
00322 for(SplitAxisType i = 0; i < static_cast<SplitAxisType>(Dimension); i++) {
00323 m_splitAxes.push_back(i);
00324 }
00325 resetStatistics();
00326 }
00327
00328 void init(const std::initializer_list<Method> & m,
00329 unsigned int allowSplitAboveNPoints = 100,
00330 PREC minExtent = 0.0,
00331 SearchCriteria searchCriteria = SearchCriteria::FIND_BEST,
00332 const QualityEvaluator & qualityEval = QualityEvaluator(),
00333 PREC minSplitRatio = 0.0, PREC minPointRatio = 0.0, PREC minExtentRatio = 0.0) {
00334
00335 m_methods = m;
00336
00337 if(m_methods.size()==0) {
00338 ApproxMVBB_ERRORMSG("No methods for splitting given!")
00339 }
00340 m_allowSplitAboveNPoints = allowSplitAboveNPoints;
00341 m_minExtent = minExtent;
00342 if( m_minExtent < 0) {
00343 ApproxMVBB_ERRORMSG("Minimal extent has wrong value!")
00344 }
00345 m_searchCriteria = searchCriteria;
00346
00347 m_qualityEval = qualityEval;
00348
00349 m_minSplitRatio = minSplitRatio;
00350 m_minPointRatio = minPointRatio;
00351 m_minExtentRatio = minExtentRatio;
00352
00353 if( m_minSplitRatio < 0 || m_minPointRatio < 0 || m_minExtentRatio <0) {
00354 ApproxMVBB_ERRORMSG("Minimal split ratio, point ratio or extent ratio have <0 values!");
00355 }
00356
00357 resetStatistics();
00358
00359 }
00360
00361 void resetStatistics() {
00362 m_splitCalls = 0;
00363 m_tries = 0;
00364 m_splits = 0;
00365
00366 m_avgSplitRatio = 0.0;
00367 m_avgPointRatio = 0.0;
00368 m_avgExtentRatio = 0.0;
00369 }
00370
00371 std::string getStatisticsString() {
00372 std::stringstream s;
00373 s<< "\t splits : " << m_splits
00374 << "\n\t avg. split ratio (0,0.5] : " << m_avgSplitRatio / m_splits
00375 << "\n\t avg. point ratio [0,0.5] : " << m_avgPointRatio/ m_splits
00376 << "\n\t avg. extent ratio (0,1] : " << m_avgExtentRatio / m_splits
00377 << "\n\t tries / calls : " << m_tries << "/" << m_splitCalls << " = " <<(PREC)m_tries/m_splitCalls;
00378 return s.str();
00379 }
00380
00384 std::pair<NodeDataType *, NodeDataType * >
00385 doSplit(NodeType * node, SplitAxisType & splitAxis, PREC & splitPosition) {
00386
00387 ++m_splitCalls;
00388
00389 auto * data = node->data();
00390
00391
00392 if(data->size() <= m_allowSplitAboveNPoints) {
00393 return std::make_pair(nullptr,nullptr);
00394 }
00395
00396
00397
00398 m_extent = node->aabb().extent();
00399
00400 auto biggest = [&](const SplitAxisType & a, const SplitAxisType & b) {
00401 return m_extent(a) > m_extent(b);
00402 };
00403 std::sort(m_splitAxes.begin(),m_splitAxes.end(),biggest);
00404
00405
00406 unsigned int tries = 0;
00407
00408 std::size_t nMethods = m_methods.size();
00409 std::size_t methodIdx = 0;
00410 std::size_t axisIdx = 0;
00411 m_found = false;
00412 m_bestQuality = std::numeric_limits<PREC>::lowest();
00413
00414 while(((m_searchCriteria == SearchCriteria::FIND_FIRST && !m_found) ||
00415 m_searchCriteria == SearchCriteria::FIND_BEST )
00416 && methodIdx < nMethods) {
00417
00418
00419 m_wasLastTry = false;
00420 m_method = m_methods[methodIdx];
00421 m_splitAxis = m_splitAxes[axisIdx];
00422
00423
00424
00425 if( m_extent(m_splitAxis) > 2.0* m_minExtent ) {
00426
00427
00428 if( computeSplitPosition(node)) {
00429
00430
00431 if( m_splitRatio >= m_minSplitRatio &&
00432 m_pointRatio >= m_minPointRatio &&
00433 m_extentRatio >= m_minExtentRatio) {
00434
00435 updateSolution();
00436
00437 m_found = true;
00438
00439 }
00440
00441 }
00442
00443 ++tries;
00444 } else {
00445
00446 }
00447
00448
00449
00450 if( ++axisIdx == Dimension) {
00451 axisIdx = 0;
00452 ++methodIdx;
00453 }
00454
00455 }
00456 m_tries += tries;
00457
00458 if(m_found) {
00459
00460
00461
00462 if( !(m_wasLastTry && m_bestMethod==Method::MEDIAN) ) {
00463
00464 switch(m_bestMethod) {
00465 case Method::GEOMETRIC_MEAN:
00466 case Method::MIDPOINT: {
00467 auto leftPredicate = [&](const typename PointListType::value_type & a) {
00468 return PointGetter::get(a)(m_bestSplitAxis) < m_bestSplitPosition;
00469 };
00470 m_bestSplitRightIt = std::partition(data->begin(), data->end(), leftPredicate );
00471 break;
00472 }
00473 case Method::MEDIAN: {
00474 auto less = [&](const typename PointListType::value_type & a, const typename PointListType::value_type & b) {
00475 return PointGetter::get(a)(m_bestSplitAxis) < PointGetter::get(b)(m_bestSplitAxis);
00476 };
00477 std::nth_element(data->begin(), m_bestSplitRightIt, data->end(), less );
00478 m_bestSplitPosition = PointGetter::get(*(m_bestSplitRightIt))(m_bestSplitAxis);
00479 auto leftPredicate = [&](const typename PointListType::value_type & a) {
00480 return PointGetter::get(a)(m_bestSplitAxis) < m_bestSplitPosition;
00481 };
00482 m_bestSplitRightIt = std::partition(data->begin(),m_bestSplitRightIt, leftPredicate);
00483 break;
00484 }
00485 }
00486 }
00487
00488 computeStatistics();
00489
00490
00491 splitAxis = m_bestSplitAxis;
00492 splitPosition = m_bestSplitPosition;
00493 return data->split(m_bestSplitRightIt);
00494 }
00495
00496
00497
00498 return std::make_pair(nullptr,nullptr);
00499 };
00500
00501 private:
00502
00504 bool computeSplitPosition(NodeType * node,
00505 unsigned int tries = 1) {
00506
00507 auto * data = node->data();
00508 ApproxMVBB_ASSERTMSG( data, "Node @ " << node << " has no data!")
00509
00510 auto & aabb = node->aabb();
00511 switch(m_method) {
00512 case Method::MIDPOINT: {
00513 m_splitPosition = 0.5*(aabb.m_minPoint(m_splitAxis) + aabb.m_maxPoint(m_splitAxis));
00514
00515 if(!checkPosition(aabb)) {
00516 return false;
00517 }
00518
00519 m_splitRatio = 0.5;
00520 m_extentRatio = computeExtentRatio(aabb);
00521 m_pointRatio = computePointRatio(data);
00522 m_quality = m_qualityEval.compute(m_splitRatio,m_pointRatio,m_extentRatio);
00523
00524 break;
00525 }
00526 case Method::MEDIAN: {
00527 auto beg = data->begin();
00528 m_splitRightIt = beg + data->size()/2;
00529
00530 auto less = [&](const typename PointListType::value_type & a,
00531 const typename PointListType::value_type & b) {
00532 return PointGetter::get(a)(m_splitAxis) < PointGetter::get(b)(m_splitAxis);
00533 };
00534
00535
00536 std::nth_element(beg, m_splitRightIt, data->end(), less );
00537 m_splitPosition = PointGetter::get(*(m_splitRightIt))(m_splitAxis);
00538
00539 if(!checkPosition(aabb)) {
00540 return false;
00541 }
00542
00543
00544
00545
00546
00547
00548
00549 auto leftPredicate = [&](const typename PointListType::value_type & a) {
00550 return PointGetter::get(a)(m_splitAxis) < m_splitPosition;
00551 };
00552 m_splitRightIt = std::partition(beg,m_splitRightIt, leftPredicate);
00553
00554
00555
00556
00557 m_splitRatio = computeSplitRatio(aabb);
00558 m_extentRatio = computeExtentRatio(aabb);
00559 m_pointRatio = (PREC)std::distance(beg,m_splitRightIt) / std::distance(beg,data->end());
00560
00561 m_quality = m_qualityEval.compute(m_splitRatio,m_pointRatio,m_extentRatio);
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574 break;
00575 }
00576 case Method::GEOMETRIC_MEAN: {
00577 m_splitPosition = data->getGeometricMean(m_splitAxis);
00578 if(!checkPosition(aabb)) {
00579 return false;
00580 }
00581
00582 m_splitRatio = computeSplitRatio(aabb);
00583 m_extentRatio = computeExtentRatio(aabb);
00584 m_pointRatio = computePointRatio(data);
00585 m_quality = m_qualityEval.compute(m_splitRatio,m_pointRatio,m_extentRatio);
00586 break;
00587 }
00588 }
00589 return true;
00590 }
00591
00592 inline bool checkPosition(AABB<Dimension> & aabb) {
00593 ApproxMVBB_ASSERTMSG( m_splitPosition >= aabb.m_minPoint(m_splitAxis)
00594 && m_splitPosition <= aabb.m_maxPoint(m_splitAxis),
00595 "split position wrong: " << m_splitPosition << " min: " << aabb.m_minPoint.transpose()
00596 << " max: " << aabb.m_maxPoint.transpose() )
00597
00598 if( (m_splitPosition - aabb.m_minPoint(m_splitAxis)) <= m_minExtent ||
00599 (aabb.m_maxPoint(m_splitAxis) - m_splitPosition) <= m_minExtent ) {
00600 return false;
00601 }
00602 return true;
00603 }
00604
00605 inline void updateSolution() {
00606 if( m_quality > m_bestQuality ) {
00607 m_wasLastTry = true;
00608
00609 m_bestMethod = m_method;
00610 m_bestQuality = m_quality;
00611 m_bestSplitRightIt = m_splitRightIt;
00612 m_bestSplitAxis = m_splitAxis;
00613 m_bestSplitPosition = m_splitPosition;
00614
00615 m_bestSplitRatio = m_splitRatio;
00616 m_bestPointRatio = m_pointRatio;
00617 m_bestExtentRatio = m_extentRatio;
00618 }
00619 }
00620
00621
00622 inline PREC computePointRatio(NodeDataType * data) {
00623 PREC n = 0.0;
00624 for(auto & p: *data) {
00625 if(PointGetter::get(p)(m_splitAxis) < m_splitPosition) {
00626 n+=1.0;
00627 }
00628 }
00629 n /= data->size();
00630 return (n>0.5)? 1.0-n : n ;
00631 }
00632
00633 inline PREC computeSplitRatio(AABB<Dimension> & aabb) {
00634 PREC n = (m_splitPosition - aabb.m_minPoint(m_splitAxis))
00635 / (aabb.m_maxPoint(m_splitAxis) - aabb.m_minPoint(m_splitAxis)) ;
00636 ApproxMVBB_ASSERTMSG(n>0.0 && n <=1.0," split ratio negative!, somthing wrong with splitPosition: " << n );
00637 return (n>0.5)? 1.0-n : n ;
00638 }
00639
00640 inline PREC computeExtentRatio(AABB<Dimension> & aabb) {
00641
00642 static ArrayStat<Dimension> t;
00643 t = m_extent;
00644 PREC tt = t(m_splitAxis);
00645
00646 t(m_splitAxis) = m_splitPosition - aabb.m_minPoint(m_splitAxis);
00647 PREC r = t.minCoeff() / t.maxCoeff();
00648
00649 t(m_splitAxis) = tt;
00650 t(m_splitAxis) = aabb.m_maxPoint(m_splitAxis) - m_splitPosition;
00651 r = std::min(r,t.minCoeff() / t.maxCoeff());
00652
00653 ApproxMVBB_ASSERTMSG(r > 0, "extent ratio <= 0!" );
00654 return r;
00655
00656 }
00657
00658
00659
00660 inline void computeStatistics() {
00661 ++m_splits;
00662 m_avgSplitRatio += m_bestSplitRatio;
00663 m_avgPointRatio += m_bestPointRatio;
00664 m_avgExtentRatio += m_bestExtentRatio;
00665 }
00666
00668 unsigned int m_splitCalls = 0;
00669 unsigned int m_tries = 0;
00670 unsigned int m_splits = 0;
00671 PREC m_avgSplitRatio = 0;
00672 PREC m_avgPointRatio = 0;
00673 PREC m_avgExtentRatio = 0;
00674
00675
00676 QualityEvaluator m_qualityEval;
00677
00679 ArrayStat<Dimension> m_extent;
00680 iterator m_splitRightIt;
00681 PREC m_quality = 0.0;
00682
00683 PREC m_splitRatio = 0;
00684 PREC m_pointRatio = 0;
00685 PREC m_extentRatio = 0;
00686
00688 PREC m_minSplitRatio = 0.0;
00689 PREC m_minPointRatio = 0.0;
00690 PREC m_minExtentRatio = 0.0;
00691
00692 Method m_method;
00693 PREC m_splitPosition = 0.0;
00694 SplitAxisType m_splitAxis;
00695
00696
00698 bool m_wasLastTry = false;
00699 bool m_found = false;
00700 iterator m_bestSplitRightIt;
00701 PREC m_bestQuality = std::numeric_limits<PREC>::lowest();
00702 PREC m_bestSplitRatio = 0, m_bestPointRatio = 0, m_bestExtentRatio = 0;
00703 Method m_bestMethod;
00704 PREC m_bestSplitPosition = 0.0;
00705 SplitAxisType m_bestSplitAxis;
00706
00708 std::vector<SplitAxisType> m_splitAxes;
00709 std::vector<Method> m_methods;
00710 SearchCriteria m_searchCriteria;
00711
00712 std::size_t m_allowSplitAboveNPoints = 100;
00713 PREC m_minExtent = 0.0;
00714
00715 };
00716
00717
00719 template<typename T> class TreeBase;
00720 template<typename T> class TreeSimple;
00721 template<typename T> class Tree;
00722
00723
00724 #define DEFINE_KDTREE_BASENODETYPES( _Base_ ) \
00725 using SplitAxisType = typename _Base_::SplitAxisType; \
00726 using BoundaryInfoType = typename _Base_::BoundaryInfoType;
00727
00730 template<typename TDerivedNode, unsigned int Dimension>
00731 class NodeBase {
00732 private:
00733
00734 ApproxMVBB_STATIC_ASSERT( Dimension <= std::numeric_limits<char>::max())
00735
00736 template<typename D, unsigned int Dim>
00737 friend class NodeBase;
00738
00739 public:
00740
00741 using DerivedNode = TDerivedNode;
00742 using SplitAxisType = char;
00743
00744 private:
00745 class BoundaryInformation {
00746 public:
00747 static const unsigned int size = Dimension * 2;
00748
00749
00750 inline DerivedNode* & at(unsigned int idx){
00751 ApproxMVBB_ASSERTMSG(idx < size, "Index " << idx << "out of bound!");
00752 return m_nodes[idx];
00753 }
00754 inline DerivedNode* const & at(unsigned int idx) const{
00755 ApproxMVBB_ASSERTMSG(idx < size, "Index " << idx << "out of bound!");
00756 return m_nodes[idx];
00757 }
00758
00759 inline DerivedNode* & at(unsigned int axis, char minMax){
00760 ApproxMVBB_ASSERTMSG(minMax*Dimension + axis < size, "Index " << minMax*Dimension + axis << "out of bound!");
00761 return m_nodes[minMax*Dimension + axis];
00762 }
00763 inline DerivedNode* const & at(unsigned int axis, char minMax) const{
00764 ApproxMVBB_ASSERTMSG(minMax*Dimension + axis < size, "Index " << minMax*Dimension + axis << "out of bound!");
00765 return m_nodes[minMax*Dimension + axis];
00766 }
00767
00768 DerivedNode* * begin(){ return m_nodes;}
00769 DerivedNode* * end(){ return m_nodes + size;}
00770
00771 DerivedNode* const * begin() const { return m_nodes;}
00772 DerivedNode* const * end() const { return m_nodes + size;}
00773
00774 private:
00775 DerivedNode* m_nodes[size] = {nullptr};
00776 };
00777
00778 public:
00779 using BoundaryInfoType = BoundaryInformation;
00780
00781 NodeBase(){}
00782
00783 NodeBase(std::size_t idx, const AABB<Dimension> & aabb, unsigned int treeLevel = 0)
00784 : m_idx(idx), m_treeLevel(treeLevel), m_aabb(aabb) {
00785 }
00786
00787 ~NodeBase() {}
00788
00794 template<typename Derived>
00795 NodeBase(const NodeBase<Derived,Dimension> & n):
00796 m_idx(n.m_idx),m_treeLevel(n.m_treeLevel), m_aabb(n.m_aabb),
00797 m_splitAxis(n.m_splitAxis),m_splitPosition(n.m_splitPosition),
00798 m_child{{nullptr,nullptr}}, m_parent{nullptr}
00799 {}
00801 template<typename Derived>
00802 NodeBase(NodeBase<Derived,Dimension> && n):
00803 m_idx(n.m_idx), m_treeLevel(n.m_treeLevel),
00804 m_aabb(std::move(n.m_aabb)),m_splitAxis(n.m_splitAxis),
00805 m_splitPosition(n.m_splitPosition) , m_child(n.child), m_parent(n.parent)
00806 {
00807 n.m_parent = nullptr;
00808 n.m_child = {{nullptr,nullptr}};
00809 }
00810
00811 inline void setChilds(DerivedNode * r, DerivedNode * l){
00812 m_child[0]=r;
00813 m_child[1]=l;
00814 }
00815
00816 inline DerivedNode * parent(){ return m_parent;}
00817 inline const DerivedNode * parent() const { return m_parent;}
00818
00819 inline DerivedNode * leftNode() {
00820 return m_child[0];
00821 }
00822 inline const DerivedNode * leftNode() const {
00823 return m_child[0];
00824 }
00825
00826 inline DerivedNode * rightNode() {
00827 return m_child[1];
00828 }
00829 inline const DerivedNode * rightNode()const {
00830 return m_child[1];
00831 }
00832
00833 inline AABB<Dimension> & aabb() {
00834 return m_aabb;
00835 }
00836 inline const AABB<Dimension> & aabb() const {
00837 return m_aabb;
00838 }
00839
00840 inline bool hasLeftChildren() const {
00841 return m_child[0];
00842 }
00843 inline bool hasChildren() const {
00844 return m_child[0] && m_child[1];
00845 }
00846
00847 inline bool isLeaf() const {
00848 return (m_splitAxis == -1);
00849 }
00850
00851 inline void setIdx(std::size_t i){
00852 m_idx = i;
00853 }
00854
00855 inline std::size_t getIdx() const {
00856 return m_idx;
00857 }
00858
00859 inline void setSplitAxis(SplitAxisType splitAxis) {
00860 m_splitAxis= splitAxis ;
00861 }
00862 inline void setSplitPosition(PREC splitPos) {
00863 m_splitPosition= splitPos ;
00864 }
00865
00866 inline SplitAxisType getSplitAxis() const {
00867 return m_splitAxis ;
00868 }
00869 inline PREC getSplitPosition() const {
00870 return m_splitPosition;
00871 }
00872 inline PREC getSplitRatio() const {
00873 return (m_splitPosition - m_aabb.m_minPoint(m_splitAxis) )
00874 / (m_aabb.m_maxPoint(m_splitAxis)-m_aabb.m_minPoint(m_splitAxis));
00875 }
00876
00877 inline unsigned int getLevel() const {
00878 return m_treeLevel;
00879 }
00880
00881 inline void setLevel(unsigned int l) const {
00882 m_treeLevel = l;
00883 }
00884
00885 protected:
00886
00887 NodeBase(std::size_t idx, const AABB<Dimension> & aabb, SplitAxisType axis, PREC splitPos)
00888 : m_idx(idx), m_aabb(aabb), m_splitAxis(axis), m_splitPosition(splitPos) {
00889 }
00890
00891 std::size_t m_idx = std::numeric_limits<std::size_t>::max();
00892 unsigned int m_treeLevel = 0;
00893 AABB<Dimension> m_aabb;
00894 SplitAxisType m_splitAxis = -1;
00895 PREC m_splitPosition = 0.0;
00896
00901 std::array<DerivedNode *,2> m_child{{nullptr,nullptr}};
00902 DerivedNode * m_parent = nullptr;
00903 };
00904
00906 template<typename Traits> class Node;
00907
00908 namespace details{
00909
00910 template<typename PD,typename T>
00911 struct select{ using type = PD;};
00912
00913 template<typename T>
00914 struct select<void,T>{ using type = T; };
00915 }
00916
00917 template<typename TTraits, typename PD = void>
00918 class NodeSimple : public NodeBase< typename details::select<PD, NodeSimple<TTraits,PD> >::type ,
00919 TTraits::Dimension>
00920 {
00921 public:
00922 using Traits = TTraits;
00923
00924 using Base = NodeBase< typename details::select<PD, NodeSimple<TTraits,PD> >::type , TTraits::Dimension>;
00925 using DerivedType = typename details::select<PD, NodeSimple<TTraits,PD> >::type;
00926
00927 protected:
00928 using Base::m_idx;
00929 using Base::m_aabb;
00930 using Base::m_splitAxis;
00931 using Base::m_splitPosition;
00932 using Base::m_child;
00933
00934 template<typename T>
00935 friend class TreeBase;
00936
00937
00941 typename Base::BoundaryInfoType m_bound;
00942
00943 public:
00944
00945 DEFINE_KDTREE_BASETYPES( Traits )
00946 DEFINE_KDTREE_BASENODETYPES( Base )
00947
00948 NodeSimple(){}
00949 NodeSimple(std::size_t idx, const AABB<Dimension> & aabb): Base(idx,aabb) {}
00950 NodeSimple(NodeSimple&& t): Base(std::move(t)) {}
00951 NodeSimple(const NodeSimple& t): Base(t) {}
00952
00954 template<typename T>
00955 explicit NodeSimple(const Node<T> & t): Base(t) {
00956
00957 }
00958
00962 template<typename T, typename NodeVector>
00963 void setup( const Node<T> * t, const NodeVector & nodes){
00964
00965 ApproxMVBB_STATIC_ASSERT(Dimension==Node<T>::Dimension);
00966
00967
00968
00969 Node<T> * c = t->m_child[0];
00970 if(c){
00971 m_child[0] = nodes[c->getIdx()];
00972 m_child[0]->m_parent = static_cast<DerivedType*>(this);
00973 }
00974
00975
00976 c = t->m_child[1];
00977 if(c){
00978 m_child[1] = nodes[c->getIdx()];
00979 m_child[1]->m_parent = static_cast<DerivedType*>(this);
00980 }
00981
00982
00983
00984 if(t->isLeaf()){
00985 auto it = m_bound.begin();
00986 for(const auto * bNode : t->m_bound){
00987
00988 if( bNode ){
00989
00990 auto * p = nodes[bNode->getIdx()];
00991 if(p == nullptr){
00992 ApproxMVBB_ERRORMSG("Setup in node: " << this->getIdx() << " failed!");
00993 }
00994 *it = p;
00995 }else{
00996 *it = nullptr;
00997 }
00998
00999 ++it;
01000 }
01001 }
01002
01003 }
01004
01005 const BoundaryInfoType & getBoundaries() const {return m_bound;}
01006 BoundaryInfoType & getBoundaries() {return m_bound;}
01007 };
01008
01010 template<unsigned int Dim = 3>
01011 struct NoData {
01012 static const unsigned int Dimension = Dim;
01013 };
01014
01015
01016 template<typename TTraits>
01017 class Node : public NodeBase<Node<TTraits>,TTraits::Dimension> {
01018 public:
01019 using Traits = TTraits;
01020 using Base = NodeBase<Node<TTraits>,TTraits::Dimension>;
01021 private:
01022
01023 using Base::m_idx;
01024 using Base::m_aabb;
01025 using Base::m_splitAxis;
01026 using Base::m_splitPosition;
01027 using Base::m_child;
01028
01029
01030
01032 template<typename T>
01033 friend class Tree;
01034 template<typename T>
01035 friend class TreeBase;
01036
01038 template<typename T>
01039 friend class Node;
01040 template<typename T,typename PD>
01041 friend class NodeSimple;
01042
01043 public:
01044
01045 DEFINE_KDTREE_BASETYPES( Traits )
01046 DEFINE_KDTREE_BASENODETYPES( Base )
01047
01048
01049
01050 Node(std::size_t idx, const AABB<Dimension> & aabb, NodeDataType * data, unsigned int treeLevel = 0)
01051 : Base(idx,aabb,treeLevel), m_data(data) {
01052 }
01053
01054 ~Node() {
01055 cleanUp();
01056 }
01057
01063 template<typename Traits>
01064 Node(const Node<Traits> & n): Base(n) {
01065 if(n.m_data) {
01066 m_data = new NodeDataType(*n.m_data);
01067 }
01068 }
01069
01071 Node(Node && n): Base(std::move(n)),
01072 m_bound(n.m_bound),
01073 m_data(n.m_data) {
01074 n.m_data = nullptr;
01075 }
01076
01077 const BoundaryInfoType & getBoundaries() const {return m_bound;}
01078 BoundaryInfoType & getBoundaries() {return m_bound;}
01079
01080 void setBoundaryInfo(const BoundaryInfoType & b) {
01081 m_bound = b;
01082 }
01083
01084 inline NodeDataType * data() {
01085 return m_data;
01086 }
01087 inline const NodeDataType * data() const {
01088 return m_data;
01089 }
01090
01096 template<typename TSplitHeuristic>
01097 bool split(TSplitHeuristic & s, std::size_t startIdx = 0) {
01098
01099 auto pLR = s.doSplit(this, m_splitAxis, m_splitPosition);
01100
01101 if(pLR.first == nullptr) {
01102 return false;
01103 }
01104
01105
01106 startIdx = (startIdx == 0) ? 2*m_idx + 1 : startIdx;
01107
01108
01109
01110 AABB<Dimension> t(m_aabb);
01111 PREC v = t.m_maxPoint(m_splitAxis);
01112 t.m_maxPoint(m_splitAxis) = m_splitPosition;
01113 m_child[0] = new Node(startIdx,t,pLR.first, this->m_treeLevel+1);
01114 m_child[0]->m_parent = this;
01115
01116
01117 t.m_maxPoint(m_splitAxis) = v;
01118 t.m_minPoint(m_splitAxis) = m_splitPosition;
01119 m_child[1] = new Node(startIdx+1,t,pLR.second, this->m_treeLevel+1);
01120 m_child[1]->m_parent = this;
01121
01122
01123 BoundaryInfoType b = m_bound;
01124 Node * tn = b.at(m_splitAxis,1);
01125 b.at(m_splitAxis,1) = m_child[1];
01126 m_child[0]->setBoundaryInfo(b);
01127
01128 b.at(m_splitAxis,1) = tn;
01129 b.at(m_splitAxis,0) = m_child[0];
01130 m_child[1]->setBoundaryInfo(b);
01131
01132
01133 if(this->m_treeLevel != 0) {
01134 cleanUp();
01135 }
01136
01137 return true;
01138 }
01139
01140 template<typename NeighbourIdxMap>
01141 void getNeighbourLeafsIdx( NeighbourIdxMap & neigbourIdx, PREC minExtent) {
01142
01143
01144
01145
01146
01147
01148
01149
01150
01151
01152
01153
01154
01155
01156
01157
01158
01159
01160
01161
01162
01163 std::deque<Node*> nodes;
01164 auto & neighbours = neigbourIdx[m_idx];
01165 Node * f;
01166
01167 AABB<Dimension> aabb(m_aabb);
01168 aabb.expand(minExtent);
01169
01170
01171 for(SplitAxisType d = 0; d< static_cast<SplitAxisType>(Dimension); ++d) {
01172
01173
01174 for(unsigned int m = 0; m<2; ++m) {
01175
01176
01177 Node * subTreeRoot = m_bound.at(d,m);
01178 if(!subTreeRoot) {
01179 continue;
01180 } else {
01181 nodes.emplace_back(subTreeRoot);
01182 }
01183
01184 while(!nodes.empty()) {
01185
01186 f = nodes.front();
01187 ApproxMVBB_ASSERTMSG(f, "Node f is nullptr")
01188
01189 auto axis = f->m_splitAxis;
01190 if(f->isLeaf()) {
01191
01192
01193
01194 if( neighbours.find(f->m_idx) == neighbours.end() ) {
01195
01196
01197 if(aabb.overlapsSubSpace(f->m_aabb,d)) {
01198
01199 neighbours.emplace(f->m_idx);
01200 neigbourIdx[f->m_idx].emplace(m_idx);
01201 }
01202
01203 }
01204
01205 } else {
01206
01207
01208 if(axis == d) {
01209 if(m==0) {
01210
01211 nodes.emplace_back(f->rightNode());
01212 } else {
01213
01214 nodes.emplace_back(f->leftNode());
01215 }
01216 } else {
01217
01218 nodes.emplace_back(f->leftNode());
01219 nodes.emplace_back(f->rightNode());
01220 }
01221 }
01222 nodes.pop_front();
01223 }
01224
01225
01226 }
01227
01228 }
01229
01230 }
01231
01232 void cleanUp(bool data = true ) {
01233 if(data && m_data) {
01234 delete m_data;
01235 m_data = nullptr;
01236 }
01237 }
01238
01239 std::size_t size() {
01240 return (m_data)? m_data->size() : 0;
01241 }
01242
01243 private:
01244
01245 NodeDataType* m_data = nullptr;
01246
01250 BoundaryInfoType m_bound;
01251
01252 };
01258 template<typename Traits>
01259 class TreeBase {
01260 private:
01261 template<typename T>
01262 friend class TreeBase;
01263 public:
01264
01265 DEFINE_KDTREE_BASETYPES( Traits )
01266
01267 TreeBase() {}
01268
01269 TreeBase(TreeBase && t)
01270 : m_nodes(std::move(t.m_nodes)), m_leafs( std::move(t.m_leafs) ), m_root(t.m_root) {
01271
01272 t.m_root = nullptr;
01273 t.m_nodes.clear();
01274 t.m_leafs.clear();
01275 }
01276
01277 TreeBase(const TreeBase & t) {
01278 copyFrom(t);
01279 }
01280
01281 template<typename T>
01282 explicit TreeBase(const TreeBase<T> & t) {
01283 copyFrom(t);
01284 }
01285
01286
01287 protected:
01288
01289
01290
01297 template<typename T>
01298 void copyFrom(const TreeBase<T> & tree) {
01299
01300
01301
01302 if(!tree.m_root) {
01303 return;
01304 }
01305
01306 this->m_nodes.reserve(tree.m_nodes.size());
01307 this->m_leafs.reserve(tree.m_leafs.size());
01308
01309
01310 for( auto * n : tree.m_nodes){
01311 ApproxMVBB_ASSERTMSG(n, "Node of tree to copy from is nullptr!")
01312 this->m_nodes.emplace_back( new NodeType(*n) );
01313 }
01314
01315
01316 auto s = tree.m_nodes.size();
01317 for( std::size_t i=0;i<s;++i){
01318 this->m_nodes[i]->setup(tree.m_nodes[i],this->m_nodes);
01319 }
01320
01321
01322 for( auto * n : tree.m_leafs){
01323
01324 ApproxMVBB_ASSERTMSG( (n->getIdx() < this->m_nodes.size()) , "Leaf node from source is out of range: " << n->getIdx() <<"," << this->m_nodes.size() )
01325 this->m_leafs.emplace_back( this->m_nodes[n->getIdx()] );
01326 }
01327
01328
01329 ApproxMVBB_ASSERTMSG( tree.m_root->getIdx() < this->m_nodes.size(), "Root idx out of range: " << tree.m_root->getIdx() << "," << this->m_nodes.size() )
01330 m_root = m_nodes[ tree.m_root->getIdx() ];
01331
01332 }
01333
01334 ~TreeBase() {
01335 resetTree();
01336 }
01337
01338 public:
01339
01340
01348 template<typename NodeMap, typename NodeToChildMap>
01349 void build( NodeType * root, NodeMap & c, NodeToChildMap & links) {
01350
01351 resetTree();
01352 m_leafs.clear();
01353 m_nodes.clear();
01354 m_nodes.reserve(c.size());
01355 m_nodes.assign(c.begin(),c.end());
01356
01357 for(auto * n: m_nodes) {
01358 if(n->isLeaf()) {
01359 m_leafs.push_back(n);
01360 }
01361 }
01362
01363 if( c.find(root->getIdx()) == c.end()) {
01364 ApproxMVBB_ERRORMSG("Root node not in NodeMap!")
01365 }
01366
01367
01368 std::unordered_set<std::size_t> hasParent;
01369
01370 auto itE = c.end();
01371 for(auto & l : links) {
01372 auto it = c.find(l.first);
01373 auto itL = c.find(l.second.first);
01374 auto itR = c.find(l.second.second);
01375
01376 if(it==itE || itL==itE || itR==itE) {
01377 ApproxMVBB_ERRORMSG("Link at node idx: " << l.first << " wrong!")
01378 }
01379
01380 if(!hasParent.emplace(l.second.first).second) {
01381 ApproxMVBB_ERRORMSG("Node idx: " << l.second.first << "has already a parent!")
01382 };
01383 if(!hasParent.emplace(l.second.second).second) {
01384 ApproxMVBB_ERRORMSG("Node idx: " << l.second.first << "has already a parent!")
01385 };
01386 if( !it->second || !itL->second || !itR->second) {
01387 ApproxMVBB_ERRORMSG("Ptr for link zero")
01388 }
01389 it->second->m_child[0] = itL->second;
01390 it->second->m_child[1] = itR->second;
01391 }
01392
01393 if(hasParent.size() != c.size()-1) {
01394 ApproxMVBB_ERRORMSG("Tree needs to have N nodes, with one root, which gives N-1 parents!")
01395 }
01396
01397
01398 m_root = root;
01399
01400
01401
01402 }
01403
01404 void resetTree() {
01405 for(auto * n: this->m_nodes) {
01406 delete n;
01407 }
01408
01409 this->m_root= nullptr;
01410
01411 m_leafs.clear();
01412 m_nodes.clear();
01413 }
01414
01419 template<typename Derived>
01420 const NodeType * getLeaf(const MatrixBase<Derived> & point) const {
01421 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived,Dimension);
01422
01423 ApproxMVBB_ASSERTMSG(m_root, "Tree is not built!")
01424 const NodeType * currentNode = m_root;
01425
01426 while(!currentNode->isLeaf()) {
01427
01428 if(point(currentNode->getSplitAxis()) >= currentNode->getSplitPosition()) {
01429 currentNode = currentNode->rightNode();
01430 } else {
01431 currentNode = currentNode->leftNode();
01432 }
01433 }
01434 return currentNode;
01435 }
01436
01440 const NodeType * getLowestCommonAncestor(const NodeType * a, const NodeType * b){
01441
01442
01443 ApproxMVBB_ASSERTMSG(a && b, "Input nodes are nullptr!")
01444
01445 static std::vector<NodeType *> aL;
01446 static std::vector<NodeType *> bL;
01447 aL.clear();
01448 bL.clear();
01449
01450
01451 NodeType * r = a->m_parent;
01452 while(r != nullptr){
01453 aL.emplace_back(r);
01454
01455 r = r->m_parent;
01456 }
01457
01458
01459
01460 r = b->m_parent;
01461 while(r != nullptr){
01462 bL.emplace_back(r);
01463
01464 r = r->m_parent;
01465 }
01466
01467
01468
01469
01470
01471
01472
01473
01474
01475
01476
01477 std::size_t sizeA = aL.size();
01478 std::size_t sizeB = bL.size();
01479 NodeType * ret = nullptr;
01480 std::size_t s = std::min(sizeA,sizeB);
01481
01482 for(std::size_t i = 1; i<=s;++i){
01483 if (aL[sizeA-i] == bL[sizeB-i]){
01484 ret = aL[sizeA-i];
01485 }else{
01486 break;
01487 }
01488 }
01489 return ret;
01490 }
01491
01492 inline const NodeType * getLeaf(const std::size_t & leafIndex) const {
01493 if(leafIndex < m_leafs.size() ) {
01494 return m_leafs[leafIndex];
01495 }
01496 return nullptr;
01497 }
01498
01499 inline const LeafContainerType & getLeafs() {
01500 return m_leafs;
01501 }
01502
01503 inline const NodeType * getNode(const std::size_t & globalIndex) const {
01504 if(globalIndex < m_nodes.size()) {
01505 return m_nodes[globalIndex];
01506 }
01507 return nullptr;
01508 }
01509
01510 inline const NodeContainerType & getNodes() {
01511 return m_nodes;
01512 }
01513
01514 inline const NodeType * getRootNode(){
01515 return m_root;
01516 }
01517
01518
01522 template<typename... T>
01523 void cleanUp(T && ... t) {
01524 for(auto * p : m_nodes) {
01525 p->cleanUp(std::forward<T>(t)...);
01526 }
01527 }
01528
01529 std::tuple<std::size_t, std::size_t >
01530 getStatistics() {
01531 return std::make_tuple(m_nodes.size(),m_leafs.size());
01532 }
01533
01534
01538 void enumerateNodes() {
01539
01540 std::size_t leafIdx = 0;
01541
01542 this->m_leafs.clear();
01543
01544
01545 for(auto * n : this->m_nodes) {
01546 if(n->isLeaf()) {
01547 n->m_idx=leafIdx++;
01548 this->m_leafs.emplace_back(n);
01549 }
01550 }
01551 std::size_t nonleafIdx = this->m_leafs.size();
01552
01553
01554 for(auto * n : this->m_nodes) {
01555 if(!n->isLeaf()) {
01556 n->m_idx = nonleafIdx++;
01557 }
01558 }
01559
01560
01561 std::sort(m_nodes.begin(),m_nodes.end(), [](NodeType * a, NodeType * b) {return a->getIdx()< b->getIdx() ;} );
01562
01563 }
01564
01565 friend class XML;
01566 protected:
01567
01568 NodeContainerType m_leafs;
01569 NodeContainerType m_nodes;
01570
01571 NodeType * m_root = nullptr;
01572 };
01576 class TreeStatistics {
01577 public:
01578 TreeStatistics() {
01579 reset();
01580 }
01581 TreeStatistics(const TreeStatistics & s) = default;
01582
01583 bool m_computedTreeStats;
01584 unsigned int m_treeDepth;
01585 PREC m_avgSplitPercentage;
01587 PREC m_minLeafExtent;
01588 PREC m_maxLeafExtent;
01590 PREC m_avgLeafSize;
01591 std::size_t m_minLeafDataSize;
01592 std::size_t m_maxLeafDataSize;
01593
01595 bool m_computedNeighbourStats;
01596 std::size_t m_minNeighbours;
01597 std::size_t m_maxNeighbours;
01598 PREC m_avgNeighbours;
01599
01600 void reset() {
01601 m_computedTreeStats = false;
01602 m_treeDepth = 0;
01603 m_avgSplitPercentage = 0.0;
01604 m_minLeafExtent = std::numeric_limits<PREC>::max();
01605 m_maxLeafExtent = 0.0;
01606 m_avgLeafSize = 0;
01607 m_minLeafDataSize = std::numeric_limits<std::size_t>::max();
01608 m_maxLeafDataSize = 0;
01609
01610 m_computedNeighbourStats = false;
01611 m_minNeighbours = std::numeric_limits<std::size_t>::max();
01612 m_maxNeighbours = 0;
01613 m_avgNeighbours = 0;
01614 }
01615
01616 void average(std::size_t nodes, std::size_t leafs) {
01617 if( nodes ){
01618 m_avgLeafSize /= nodes;
01619 }
01620 if(leafs){
01621 m_avgSplitPercentage /= nodes - leafs;
01622 }
01623 }
01624
01625 template<typename TNode>
01626 void addNode(TNode * n) {
01627 if(n->isLeaf()) {
01628 auto s = n->data()->size();
01629 m_avgLeafSize += s;
01630 m_minLeafDataSize = std::min(m_minLeafDataSize,s);
01631 m_maxLeafDataSize = std::max(m_maxLeafDataSize,s);
01632 m_minLeafExtent = std::min(m_minLeafExtent,n->aabb().extent().minCoeff());
01633 m_maxLeafExtent = std::max(m_maxLeafExtent,n->aabb().extent().maxCoeff());
01634 }
01635 }
01636
01637 friend class XML;
01638 };
01639
01640
01643 template<
01644 typename TNodeData = NoData<>,
01645 template<typename...> class TNode = NodeSimple
01646 >
01647 struct TreeSimpleTraits {
01648
01649 struct BaseTraits {
01650 using NodeDataType = TNodeData;
01651 static const unsigned int Dimension = NodeDataType::Dimension;
01652 using NodeType = TNode<BaseTraits>;
01653 };
01654
01655 };
01656
01657
01659 template<typename TTraits = TreeSimpleTraits<> >
01660 class TreeSimple : public TreeBase<typename TTraits::BaseTraits> {
01661 public:
01662
01663 using Traits = TTraits;
01664 using BaseTraits = typename TTraits::BaseTraits;
01665 using Base = TreeBase<typename TTraits::BaseTraits>;
01666
01667 DEFINE_KDTREE_BASETYPES(BaseTraits)
01668
01669
01670 TreeSimple(){}
01671
01673 template<typename Traits>
01674 TreeSimple( TreeSimple<Traits> && tree)
01675 : Base(std::move(tree)), m_statistics(std::move(tree.m_statistics)) {}
01677 template<typename Traits>
01678 TreeSimple( const TreeSimple<Traits> & tree)
01679 : Base(tree), m_statistics(tree.m_statistics){}
01680
01684 template< typename Traits >
01685 explicit TreeSimple( const Tree<Traits> & tree)
01686 : Base( tree ) , m_statistics(tree.m_statistics)
01687 {}
01688
01689
01690
01691 ~TreeSimple() {}
01692
01696 std::tuple<std::size_t, std::size_t, std::size_t, PREC, std::size_t, std::size_t, PREC, PREC,std::size_t,std::size_t,PREC>
01697 getStatistics() {
01698 return std::tuple_cat(
01699 Base::getStatistics(),
01700 std::make_tuple(
01701 m_statistics.m_treeDepth,
01702 m_statistics.m_avgLeafSize,
01703 m_statistics.m_minLeafDataSize,
01704 m_statistics.m_maxLeafDataSize,
01705 m_statistics.m_minLeafExtent,
01706 m_statistics.m_maxLeafExtent,
01707 m_statistics.m_minNeighbours,
01708 m_statistics.m_maxNeighbours,
01709 m_statistics.m_avgNeighbours)
01710 );
01711 }
01712
01713
01714 std::string getStatisticsString() {
01715 std::stringstream s;
01716 auto t = getStatistics();
01717 s << "Tree Stats: "
01718 << "\n\t nodes : " << std::get<0>(t)
01719 << "\n\t leafs : " << std::get<1>(t)
01720 << "\n\t tree level : " << std::get<2>(t)
01721 << "\n\t avg. leaf data size : " << std::get<3>(t)
01722 << "\n\t min. leaf data size : " << std::get<4>(t)
01723 << "\n\t max. leaf data size : " << std::get<5>(t)
01724 << "\n\t min. leaf extent : " << std::get<6>(t)
01725 << "\n\t max. leaf extent : " << std::get<7>(t)
01726 << "\nNeighbour Stats (if computed): \n"
01727 << "\n\t min. leaf neighbours : " << std::get<8>(t)
01728 << "\n\t max. leaf neighbours : " << std::get<9>(t)
01729 << "\n\t avg. leaf neighbours : " << std::get<10>(t) << std::endl;
01730
01731 return s.str();
01732 }
01733
01734
01735 friend class XML;
01736 protected:
01737
01738 using Base::m_nodes;
01739 using Base::m_leafs;
01740 using Base::m_root;
01741
01742 TreeStatistics m_statistics;
01743 };
01744
01749 template<typename Traits>
01750 using SplitHeuristicPointDataDefault = meta::invoke<meta::bind_front<
01751 meta::quote<SplitHeuristicPointData>,
01752 LinearQualityEvaluator
01753 >, Traits>;
01754
01755
01756 template<
01757 typename TNodeData = PointData<>,
01758 template<typename...> class TSplitHeuristic = SplitHeuristicPointDataDefault,
01759 template<typename...> class TNode = Node
01760 >
01761 struct TreeTraits {
01762
01763 struct BaseTraits {
01764 using NodeDataType = TNodeData;
01765 static const unsigned int Dimension = NodeDataType::Dimension;
01766 using NodeType = TNode<BaseTraits>;
01767 };
01768
01769 using SplitHeuristicType = TSplitHeuristic<BaseTraits>;
01770
01771
01772 };
01773
01774
01775
01778 template<typename TTraits = TreeTraits<> >
01779 class Tree : public TreeBase<typename TTraits::BaseTraits> {
01780 private:
01781
01782 template<typename T>
01783 friend class Tree;
01784
01785 template<typename T>
01786 friend class TreeSimple;
01787
01788 public:
01789
01790 using Traits = TTraits;
01791 using BaseTraits = typename TTraits::BaseTraits;
01792 using Base = TreeBase<typename TTraits::BaseTraits>;
01793 DEFINE_KDTREE_BASETYPES(BaseTraits)
01794
01795 using SplitHeuristicType = typename Traits::SplitHeuristicType;
01796
01797 Tree() {}
01798 ~Tree() {}
01799
01801 Tree( Tree && tree)
01802 : Base(std::move(tree)),
01803 m_heuristic(std::move(tree.m_heuristic)),
01804 m_statistics(std::move(tree.m_statistics)),
01805 m_maxLeafs(tree.m_maxLeafs), m_maxTreeDepth(tree.m_maxTreeDepth) {
01806 tree.resetStatistics();
01807 }
01808
01810 Tree( const Tree & tree): Base(tree),
01811 m_heuristic(tree.m_heuristic),
01812 m_statistics(tree.m_statistics),
01813 m_maxLeafs(tree.m_maxLeafs), m_maxTreeDepth(tree.m_maxTreeDepth) {
01814 }
01816 template<typename T>
01817 explicit Tree( const Tree<T> & tree): Base(tree),
01818 m_statistics(tree.m_statistics),
01819 m_maxLeafs(tree.m_maxLeafs), m_maxTreeDepth(tree.m_maxTreeDepth) {
01820 }
01821
01825 template<typename TTree>
01826 Tree( const TTree & tree): Base(tree) {}
01827
01828
01829
01830 Tree & operator=(const Tree & t) = delete;
01831
01832 void resetTree() {
01833 resetStatistics();
01834 Base::resetTree();
01835 }
01836
01842 template<bool computeStatistics = true>
01843 void build(const AABB<Dimension> & aabb, std::unique_ptr<NodeDataType> data,
01844 unsigned int maxTreeDepth = 50,
01845 unsigned int maxLeafs = std::numeric_limits<unsigned int>::max()) {
01846
01847 resetTree();
01848
01849
01850 m_statistics.m_computedTreeStats = computeStatistics;
01851 m_maxTreeDepth = maxTreeDepth;
01852 m_maxLeafs = maxLeafs;
01853
01854
01855 if((aabb.extent() <= 0.0).any()) {
01856 ApproxMVBB_ERRORMSG("AABB given has wrong extent!");
01857 }
01858 this->m_root = new NodeType(0,aabb,data.release());
01859
01860 std::deque<NodeType*> splitList;
01861 splitList.push_back(this->m_root);
01862 this->m_nodes.push_back(this->m_root);
01863
01864 bool nodeSplitted;
01865
01866 m_statistics.m_treeDepth = 0;
01867 unsigned int nNodesLevelCurr = 1;
01868 unsigned int nNodesLevelNext = 0;
01869
01870 unsigned int nLeafs = 1;
01871
01872
01873
01874
01875
01876
01877 while( !splitList.empty()) {
01878
01879
01880 auto * f = splitList.front();
01881
01882
01883 if(nNodesLevelCurr==0) {
01884
01885 ++m_statistics.m_treeDepth;
01886
01887 std::swap(nNodesLevelCurr, nNodesLevelNext);
01888
01889
01890
01891
01892 f = splitList.front();
01893
01894 }
01895
01896 if(m_statistics.m_treeDepth+1 <= m_maxTreeDepth && nLeafs < m_maxLeafs) {
01897
01898
01899 nodeSplitted = f->split(m_heuristic,this->m_nodes.size());
01900 if(nodeSplitted) {
01901 auto * l = f->leftNode();
01902 auto * r = f->rightNode();
01903 splitList.emplace_back(l);
01904 splitList.emplace_back(r);
01905
01906
01907 this->m_nodes.emplace_back(l);
01908 this->m_nodes.emplace_back(r);
01909
01910 nNodesLevelNext+=2;
01911 ++nLeafs;
01912
01913 } else {
01914
01915 this->m_leafs.emplace_back(f);
01916 }
01917
01918
01919 if(computeStatistics) {
01920 m_statistics.addNode(f);
01921 }
01922
01923 } else {
01924
01925
01926 this->m_leafs.emplace_back(f);
01927
01928
01929 if(computeStatistics) {
01930 m_statistics.addNode(f);
01931 }
01932
01933 }
01934 --nNodesLevelCurr;
01935
01936 splitList.pop_front();
01937 }
01938
01939
01940 this->enumerateNodes();
01941
01942
01943 if(computeStatistics) {
01944 averageStatistics();
01945 }
01946 }
01947
01948 template<typename... T>
01949 void initSplitHeuristic(T &&... t) {
01950 m_heuristic.init(std::forward<T>(t)...);
01951 }
01952
01953 template<bool computeStatistics = true, bool safetyCheck = true>
01954 LeafNeighbourMapType
01955 buildLeafNeighboursAutomatic() {
01956 if(!m_statistics.m_computedTreeStats) {
01957 ApproxMVBB_ERRORMSG("You did not compute statistics for this tree while constructing it!")
01958 }
01959
01960
01961 return buildLeafNeighbours<computeStatistics,safetyCheck>(0.99*m_statistics.m_minLeafExtent);
01962 }
01963
01964 template<bool computeStatistics = true, bool safetyCheck = true>
01965 LeafNeighbourMapType
01966 buildLeafNeighbours(PREC minExtent) {
01967 if(!this->m_root) {
01968 ApproxMVBB_ERRORMSG("There is not root node! KdTree not built!")
01969 }
01970
01971 m_statistics.m_computedNeighbourStats = computeStatistics;
01972
01973
01974
01975
01976
01977
01978
01979
01980
01981
01982
01983
01984
01985
01986
01987
01988
01989
01990
01991
01992
01993
01994
01995
01996 std::unordered_map<std::size_t, std::unordered_set<std::size_t> > leafToNeighbourIdx;
01997
01998
01999 for(auto * node: this->m_leafs) {
02000 node->getNeighbourLeafsIdx(leafToNeighbourIdx, minExtent);
02001 }
02002
02003
02004 if(safetyCheck) {
02005 safetyCheckNeighbours(leafToNeighbourIdx,minExtent);
02006 }
02007
02008
02009 if(computeStatistics) {
02010 m_statistics.m_minNeighbours = std::numeric_limits<std::size_t>::max();
02011 m_statistics.m_maxNeighbours = 0;
02012 m_statistics.m_avgNeighbours = 0;
02013
02014 for(auto & n: leafToNeighbourIdx) {
02015 m_statistics.m_avgNeighbours += n.second.size();
02016 m_statistics.m_minNeighbours = std::min(m_statistics.m_minNeighbours,n.second.size());
02017 m_statistics.m_maxNeighbours = std::max(m_statistics.m_maxNeighbours,n.second.size());
02018
02019 }
02020 m_statistics.m_avgNeighbours /= this->m_leafs.size();
02021 }
02022
02023 return leafToNeighbourIdx;
02024 }
02025
02027 struct ParentInfo {
02028 ParentInfo( NodeType* p, bool l=false,bool r = false): m_parent(p), childVisited{l,r} {}
02029 NodeType* m_parent;
02030 bool childVisited[2];
02031 };
02032
02033 private:
02034
02039 template <typename Container, typename Compare>
02040 class KNearestPrioQueue : public std::priority_queue<typename NodeDataType::PointListType::value_type,
02041 Container,
02042 Compare> {
02043 public:
02044
02045 using value_type = typename Container::value_type;
02046
02047 ApproxMVBB_STATIC_ASSERT((std::is_same< value_type,
02048 typename NodeDataType::PointListType::value_type>::value))
02049
02050 using Base = std::priority_queue<typename NodeDataType::PointListType::value_type,Container,Compare>;
02051
02052 using iterator = typename Container::iterator;
02053 using reverse_iterator = typename Container::reverse_iterator;
02054
02055 Container &getContainer() {
02056 return this->c;
02057 }
02058 Compare &getComperator() {
02059 return this->comp;
02060 }
02061
02062 iterator begin() {
02063 return this->c.begin();
02064 }
02065 iterator end() {
02066 return this->c.end();
02067 }
02068
02069 reverse_iterator rbegin() {
02070 return this->c.rbegin();
02071 }
02072 reverse_iterator rend() {
02073 return this->c.rend();
02074 }
02075
02076 KNearestPrioQueue(std::size_t maxSize): m_maxSize(maxSize) {
02077 this->c.reserve(m_maxSize);
02078 }
02079
02080 inline void clear() {
02081 this->c.clear();
02082 }
02083
02084 inline bool full() {
02085 return this->size() == m_maxSize;
02086 }
02087
02088 inline void push( const typename Base::value_type & v) {
02089 if (this->size() < m_maxSize) {
02090 Base::push(v);
02091 } else if( this->comp(v,this->top()) ) {
02092 Base::pop();
02093 Base::push(v);
02094 }
02095 }
02096
02097 template<typename It>
02098 inline void push(It beg, It end) {
02099 It it = beg;
02100 auto s = this->size();
02101 while(s < m_maxSize && it!=end) {
02102 Base::push(*it);
02103 ++it;
02104 ++s;
02105 }
02106 while(it!=end) {
02107
02108 if( this->comp(*it,this->top()) ) {
02109 Base::pop();
02110 Base::push(*it);
02111 }
02112 ++it;
02113 }
02114 }
02115
02117 template<typename Iterator>
02118 void replace(Iterator begin, Iterator end) {
02119 this->c.clear();
02120 this->c.insert(this->c.begin(),begin, end);
02121 std::make_heap(this->c.begin(), this->c.end(), this->comp );
02122 }
02123
02124 std::size_t maxSize() {
02125 return m_maxSize;
02126 }
02127
02128 private:
02129 std::size_t m_maxSize;
02130 };
02131
02132 public:
02133
02134 template<typename TDistSq = EuclideanDistSq,
02135 typename TContainer = StdVecAligned<typename NodeDataType::PointListType::value_type>
02136 >
02137 struct KNNTraits {
02138 using DistSqType = EuclideanDistSq;
02139 using ContainerType = TContainer;
02140 using DistCompType = typename NodeDataType::template DistanceComp<DistSqType>;
02141 using PrioQueue = KNearestPrioQueue<
02142 ContainerType,
02143 DistCompType
02144 >;
02145 };
02146
02147 private:
02148
02149 template<typename T> struct isKNNTraits;
02150 template<typename N, typename C>
02151 struct isKNNTraits< KNNTraits<N,C> > {
02152 static const bool value = true;
02153 };
02154
02155 public:
02156
02157 template<typename TKNNTraits>
02158 void getKNearestNeighbours( typename TKNNTraits::PrioQueue & kNearest) const {
02159
02160 ApproxMVBB_STATIC_ASSERT( isKNNTraits<TKNNTraits>::value )
02161
02162 kNearest.clear();
02163
02164 if(!this->m_root || kNearest.maxSize() == 0) {
02165 return;
02166 }
02167
02168
02169
02170 typename TKNNTraits::DistCompType & distComp = kNearest.getComperator();
02171
02172
02173
02174
02175
02176
02177 std::vector< ParentInfo > parents;
02178 parents.reserve(m_statistics.m_treeDepth);
02179
02180 parents.emplace_back( nullptr, false, false);
02181 NodeType * currNode = this->m_root;
02182
02183
02184 while(!currNode->isLeaf()) {
02185
02186 if(distComp.m_ref(currNode->m_splitAxis) >= currNode->m_splitPosition) {
02187 parents.emplace_back( currNode, false,true );
02188 currNode = currNode->rightNode();
02189 } else {
02190 parents.emplace_back( currNode, true,false );
02191 currNode = currNode->leftNode();
02192 }
02193 }
02194 ApproxMVBB_ASSERTMSG(currNode && currNode->isLeaf(), "currNode is nullptr!")
02195
02196
02197 PREC d = 0.0;
02198 PREC maxDistSq = 0.0;
02199 ParentInfo * currParentInfo = nullptr;
02200
02201
02202 while(currNode!=nullptr) {
02203
02204 currParentInfo = &parents.back();
02205 ApproxMVBB_ASSERTMSG(currNode, "currNode is nullptr!")
02206
02207 if( !currNode->isLeaf()) {
02208
02209
02210 if (!currParentInfo->childVisited[0]) {
02211
02212
02213 currParentInfo->childVisited[0] = true;
02214
02215 if( kNearest.full()) {
02216
02217 d = currParentInfo->m_parent->m_splitPosition - distComp.m_ref(currParentInfo->m_parent->m_splitAxis);
02218 if(d<=0.0) {
02219
02220 if( d*d >= maxDistSq) {
02221 continue;
02222 }
02223 }
02224
02225 }
02226
02227
02228 currNode = currNode->leftNode();
02229 ApproxMVBB_ASSERTMSG(currNode,"cannot be nullptr, since a leaf")
02230 if (!currNode->isLeaf()) {
02231
02232 parents.emplace_back(currNode);
02233 }
02234
02235 continue;
02236
02237 } else if (!currParentInfo->childVisited[1]) {
02238
02239
02240 currParentInfo->childVisited[1] = true;
02241 if( kNearest.full()) {
02242 d = currParentInfo->m_parent->m_splitPosition - distComp.m_ref(currParentInfo->m_parent->m_splitAxis);
02243 if( d > 0) {
02244
02245 if( d*d > maxDistSq) {
02246 continue;
02247 }
02248 }
02249 }
02250
02251
02252
02253 currNode = currNode->rightNode();
02254 ApproxMVBB_ASSERTMSG(currNode,"cannot be nullptr, since a leaf")
02255 if (!currNode->isLeaf()) {
02256
02257 parents.emplace_back( currNode );
02258 }
02259 continue;
02260 }
02261
02262
02263
02264 parents.pop_back();
02265
02266 currNode = parents.back().m_parent;
02267
02268 } else {
02269
02270
02271
02272
02273
02274
02275 if(currNode->size()>0) {
02276 kNearest.push(currNode->data()->begin(),currNode->data()->end());
02277
02278 maxDistSq = distComp(kNearest.top());
02279 }
02280
02281 currNode = currParentInfo->m_parent;
02282 continue;
02283 }
02284
02285
02286 }
02287 }
02288
02294 std::tuple<std::size_t, std::size_t, std::size_t, PREC, std::size_t, std::size_t, PREC, PREC,std::size_t,std::size_t,PREC>
02295 getStatistics() {
02296 return std::tuple_cat(
02297 Base::getStatistics(),
02298 std::make_tuple(
02299 m_statistics.m_treeDepth,
02300 m_statistics.m_avgLeafSize,
02301 m_statistics.m_minLeafDataSize,
02302 m_statistics.m_maxLeafDataSize,
02303 m_statistics.m_minLeafExtent,
02304 m_statistics.m_maxLeafExtent,
02305 m_statistics.m_minNeighbours,
02306 m_statistics.m_maxNeighbours,
02307 m_statistics.m_avgNeighbours)
02308 );
02309 }
02310
02311 std::string getStatisticsString() {
02312 std::stringstream s;
02313 auto t = getStatistics();
02314 std::string h = m_heuristic.getStatisticsString();
02315 s << "Tree Stats: "
02316 << "\n\t nodes : " << std::get<0>(t)
02317 << "\n\t leafs : " << std::get<1>(t)
02318 << "\n\t tree level : " << std::get<2>(t)
02319 << "\n\t avg. leaf data size : " << std::get<3>(t)
02320 << "\n\t min. leaf data size : " << std::get<4>(t)
02321 << "\n\t max. leaf data size : " << std::get<5>(t)
02322 << "\n\t min. leaf extent : " << std::get<6>(t)
02323 << "\n\t max. leaf extent : " << std::get<7>(t)
02324 << "\nSplitHeuristics Stats: \n"
02325 << h
02326 << "\nNeighbour Stats (if computed): \n"
02327 << "\n\t min. leaf neighbours : " << std::get<8>(t)
02328 << "\n\t max. leaf neighbours : " << std::get<9>(t)
02329 << "\n\t avg. leaf neighbours : " << std::get<10>(t) << std::endl;
02330
02331 return s.str();
02332 }
02333
02334 friend class XML;
02335 private:
02336
02337
02338 SplitHeuristicType m_heuristic;
02339
02340 unsigned int m_maxTreeDepth = 50;
02341 unsigned int m_maxLeafs = std::numeric_limits<unsigned int>::max();
02342
02344 TreeStatistics m_statistics;
02345
02346
02347 void resetStatistics() {
02348 m_statistics.reset();
02349 m_heuristic.resetStatistics();
02350 }
02351 void averageStatistics() {
02352 m_statistics.average(this->m_nodes.size(),this->m_leafs.size());
02353 }
02354
02355
02357 template<typename NeighbourMap>
02358 void safetyCheckNeighbours(const NeighbourMap & n, PREC minExtent) {
02359
02360 if(n.size() != this->m_leafs.size()) {
02361 ApproxMVBB_ERRORMSG("Safety check for neighbours failed!: size:" << n.size() <<","<<this->m_leafs.size())
02362 }
02363
02364
02365 for(auto * l: this->m_leafs) {
02366
02367 AABB<Dimension> t = l->aabb();
02368 t.expand(minExtent);
02369
02370
02371 auto it = n.find(l->getIdx());
02372 if(it == n.end()) {
02373 ApproxMVBB_ERRORMSG("Safety check: Leaf idx" << l->getIdx() << " not in neighbour map!")
02374 }
02375
02376 for(const auto & idx : it->second ) {
02377 if(this->getLeaf(idx) == nullptr) {
02378 ApproxMVBB_ERRORMSG("Safety check: Neighbour idx" << idx << " not found!")
02379 }
02380
02381 if( ! t.overlaps( this->m_leafs[idx]->aabb() ) ) {
02382 ApproxMVBB_ERRORMSG("Safety check: Leaf idx: " << idx << " does not overlap " << l->getIdx())
02383 }
02384
02385 auto nIt = n.find(idx);
02386 if(nIt == n.end()) {
02387 ApproxMVBB_ERRORMSG("Safety check: Neighbour idx" << idx << " not in neighbour map!")
02388 }
02389 if(nIt->second.find(l->getIdx()) == nIt->second.end() ) {
02390 ApproxMVBB_ERRORMSG("Safety check: Neighbour idx" << idx << " does not have leaf idx: " << l->getIdx() << " as neighbour")
02391 }
02392 }
02393
02394
02395 std::unordered_set<std::size_t> ns;
02396 for(auto * ll : this->m_leafs){
02397 if( ll->getIdx() != l->getIdx() && t.overlaps( ll->aabb() ) ) {
02398 ns.emplace(ll->getIdx());
02399 }
02400 }
02401
02402 for(auto & i : ns){
02403 if( it->second.find(i) == it->second.end() ){
02404 ApproxMVBB_ERRORMSG("Safety check: Bruteforce list has neighbour idx: " << i << " for leaf idx: " << l->getIdx() << " but not computed list!")
02405 }
02406 }
02407 if(ns.size() != it->second.size()){
02408 ApproxMVBB_ERRORMSG("Safety check: Bruteforce list and computed list are not the same size!" << ns.size() << "," << it->second.size() )
02409 }
02410
02411 }
02412
02413 }
02414 };
02415
02418 template<typename TTraits = KdTree::DefaultPointDataTraits<> >
02419 class NearestNeighbourFilter {
02420 public:
02421
02422 using PointDataTraits = TTraits;
02423 static const unsigned int Dim = PointDataTraits::Dimension;
02424 using PointListType = typename PointDataTraits::PointListType;
02425 using PointType = typename PointDataTraits::PointType;
02426 using PointGetter = typename PointDataTraits::PointGetter;
02427
02428 using Tree = KdTree::Tree< KdTree::TreeTraits<
02429 KdTree::PointData<PointDataTraits>
02430 >
02431 >;
02432 using SplitHeuristicType = typename Tree::SplitHeuristicType;
02433 using NodeDataType = typename Tree::NodeDataType;
02434
02436 NearestNeighbourFilter( std::size_t kNeighboursMean = 20,
02437 PREC stdDevMult=1.0,
02438 std::size_t allowSplitAboveNPoints = 10 ):
02439 m_kNeighboursMean(kNeighboursMean), m_stdDevMult(stdDevMult), m_allowSplitAboveNPoints(allowSplitAboveNPoints) {
02440 if(m_kNeighboursMean==0) {
02441 ApproxMVBB_ERRORMSG("kNeighboursMean is zero! (needs to be >=1)")
02442 }
02443 }
02444
02445 inline std::tuple<std::size_t,PREC,std::size_t>
02446 getSettings()
02447 {
02448 return std::make_tuple(m_kNeighboursMean,m_stdDevMult,m_allowSplitAboveNPoints);
02449 }
02450
02451 inline void setSettings(std::size_t kNeighboursMean,
02452 PREC stdDevMult,
02453 std::size_t allowSplitAboveNPoints)
02454 {
02455 m_kNeighboursMean = kNeighboursMean;
02456 m_stdDevMult = stdDevMult;
02457 m_allowSplitAboveNPoints = allowSplitAboveNPoints;
02458 }
02459
02468 template<typename Container,
02469 typename DistSq = EuclideanDistSq,
02470 typename = typename std::enable_if<ContainerTags::has_randomAccessIterator<Container>::value>::type
02471 >
02472 void filter(Container & points, const AABB<Dim> & aabb, Container & output, bool invert=false) {
02473
02474 ApproxMVBB_STATIC_ASSERTM( (std::is_same< typename Container::value_type,
02475 typename PointListType::value_type>::value),
02476 "Container value_type needs to be the same as value_type of PointDataTraits!")
02477
02478
02479 Tree tree;
02480
02481 typename SplitHeuristicType::QualityEvaluator qual(0.0,
02482 2.0,
02483 1.0);
02484
02485 PREC minExtent = 0.0;
02486 tree.initSplitHeuristic( std::initializer_list<typename SplitHeuristicType::Method> {
02487
02488
02489 SplitHeuristicType::Method::MIDPOINT
02490 },
02491 m_allowSplitAboveNPoints, minExtent,
02492 SplitHeuristicType::SearchCriteria::FIND_FIRST, qual,
02493 0.0, 0.0, 0.1);
02494
02495 auto rootData = std::unique_ptr<NodeDataType>(new NodeDataType(points.begin(),points.end()));
02496 unsigned int inf = std::numeric_limits<unsigned int>::max();
02497 tree.build(aabb,std::move(rootData), inf , inf );
02498
02499
02500
02501 using KNNTraits = typename Tree::template KNNTraits<DistSq>;
02502 typename KNNTraits::PrioQueue kNearest(m_kNeighboursMean+1);
02503 typename KNNTraits::DistCompType & compDist = kNearest.getComperator();
02504
02505
02506
02507 m_nearestDists.assign(points.size(),0.0);
02508
02509
02510 std::size_t validPoints = 0;
02511 typename KNNTraits::PrioQueue::iterator it;
02512 typename KNNTraits::PrioQueue::iterator e;
02513 PREC sum = 0.0;
02514 auto nPoints = points.size();
02515 for(std::size_t i = 0; i < nPoints; ++i) {
02516
02517
02518 kNearest.getComperator().m_ref = PointGetter::get(points[i]);
02519 tree.template getKNearestNeighbours<KNNTraits>(kNearest);
02520
02521
02522
02523
02524
02525
02526
02527 if( kNearest.size() > 1) {
02528 e = kNearest.end();
02529 sum = 0.0;
02530 for(it=++kNearest.begin(); it!=e; ++it) {
02531 sum += std::sqrt(compDist(*it));
02532 }
02533
02534 m_nearestDists[i] = sum / (kNearest.size()-1);
02535 ++validPoints;
02536 }
02537
02538 }
02539
02540
02541 sum = 0.0;
02542 PREC sumSq = 0.0;
02543 for (std::size_t i = 0; i < nPoints; ++i) {
02544 sum += m_nearestDists[i];
02545 sumSq += m_nearestDists[i] * m_nearestDists[i];
02546 }
02547 m_mean = sum / validPoints;
02548 m_stdDev = std::sqrt( (sumSq - sum*sum/validPoints) / validPoints );
02549
02550
02551 PREC distanceThreshold = m_mean + m_stdDevMult * m_stdDev;
02552
02553
02554
02555 output.resize(points.size());
02556 std::size_t nPointsOut = 0;
02557 if(!invert) {
02558 for(std::size_t i = 0; i < nPoints; ++i) {
02559 if (m_nearestDists[i] < distanceThreshold) {
02560
02561 output[nPointsOut] = points[i];
02562 ++nPointsOut;
02563 }
02564 }
02565 } else {
02566 for(std::size_t i = 0; i < nPoints; ++i) {
02567 if (m_nearestDists[i] >= distanceThreshold) {
02568
02569 output[nPointsOut] = points[i];
02570 ++nPointsOut;
02571 }
02572 }
02573 }
02574 output.resize(nPointsOut);
02575
02576
02577
02578 }
02579
02586 using NearestDistancesType = std::vector<PREC>;
02587 std::vector<PREC> & getNearestDists() {
02588 return m_nearestDists;
02589 }
02590
02594 std::pair<PREC,PREC> getMoments(){
02595 return std::make_pair(m_mean,m_stdDev);
02596 }
02597
02598
02599 private:
02600
02601 NearestDistancesType m_nearestDists;
02602 PREC m_mean = 0;
02603 PREC m_stdDev = 0;
02604
02606 std::size_t m_kNeighboursMean;
02611 PREC m_stdDevMult;
02612
02613 std::size_t m_allowSplitAboveNPoints;
02614
02615
02616 };
02617
02618 }
02619
02620 }
02621
02622 #endif