Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/RebuildAACubeTreeStrategy.h

raw
1 #ifndef REBUILDAACUBETREESTRATEGY_HEADER
2 #define REBUILDAACUBETREESTRATEGY_HEADER
3 #include <GfxTL/NullClass.h>
4 #include <GfxTL/VectorXD.h>
5 #include <limits>
6
7 template< class InheritedStrategyT >
8 struct RebuildAACubeTreeStrategy
9 {
10 typedef typename InheritedStrategyT::value_type value_type;
11
12 class CellData
13 : public InheritedStrategyT::CellData
14 {};
15
16 template< class BaseT >
17 class StrategyBase
18 : public InheritedStrategyT::template StrategyBase< BaseT >
19 {
20 public:
21 typedef typename InheritedStrategyT::template StrategyBase< BaseT >
22 BaseType;
23 typedef typename BaseType::CellType CellType;
24 typedef typename GfxTL::ScalarTypeDeferer< value_type >::ScalarType
25 ScalarType;
26 typedef GfxTL::VectorXD< CellType::Dim, ScalarType > PointType;
27
28 size_t Rebuild()
29 {
30 if(!BaseType::Root())
31 return 0;
32 BaseType::Root()->Range() = typename BaseType::CellRange(
33 BaseType::BeginHandle(), BaseType::EndHandle());
34 //BaseType::Root()->Size(BaseType::size());
35 if(!BaseType::Root()->Size() || BaseType::Root()->Size() < BaseType::MaxBucketSize())
36 {
37 for(unsigned int i = 0; i < CellType::NChildren; ++i)
38 {
39 if(this->ExistChild(*BaseType::Root(), i))
40 delete &((*BaseType::Root())[i]);
41 BaseType::Root()->Child(i, NULL);
42 }
43 }
44 if(this->IsLeaf(*BaseType::Root()))
45 return 0;
46 typename BaseType::HandleType cur = BaseType::BeginHandle();
47 size_t maxDepth = 0;
48 PointType min, max;
49 for(unsigned int i = 0; i < BaseType::m_dim; ++i)
50 {
51 min[i] = -std::numeric_limits< ScalarType >::infinity();
52 max[i] = std::numeric_limits< ScalarType >::infinity();
53 }
54 for(unsigned int i = 0; i < CellType::NChildren; ++i)
55 {
56 if(!this->ExistChild(*BaseType::Root(), i))
57 continue;
58 PointType cmin, cmax;
59 for(unsigned int j = 0; j < BaseType::m_dim; ++j)
60 {
61 if(i & (1 << (BaseType::m_dim - j - 1)))
62 {
63 cmin[j] = BaseType::Root()->Center()[j];
64 cmax[j] = max[j];
65 }
66 else
67 {
68 cmin[j] = min[j];
69 cmax[j] = BaseType::Root()->Center()[j];
70 }
71 }
72 size_t d = Rebuild(*BaseType::Root(), i, cmin, cmax, &cur);
73 if(d > maxDepth)
74 maxDepth = d;
75 }
76 BaseType::Root()->Range() = typename BaseType::CellRange(
77 BaseType::BeginHandle(), cur);
78 return maxDepth;
79 };
80
81 private:
82 size_t Rebuild(CellType &parent, size_t childIdx, const PointType &min,
83 const PointType &max, typename BaseType::HandleType *cur)
84 {
85 CellType &cell = parent[static_cast<unsigned>(childIdx)];
86 if(this->IsLeaf(cell))
87 {
88 typename BaseType::HandleType h = *cur;
89 if(h >= BaseType::EndHandle())
90 {
91 cell.Range() = typename BaseType::CellRange(h, h);
92 return cell.Level();
93 }
94 size_t s = cell.Size();
95 for(size_t i = 0; i < s && h < BaseType::EndHandle(); ++i, ++h)
96 {
97 size_t dref = this->Dereference(h);
98 bool inside = true;
99 for(unsigned int j = 0; j < BaseType::m_dim; ++j)
100 {
101 if(BaseType::at(dref)[j] <= min[j] || BaseType::at(dref)[j] > max[j])
102 {
103 inside = false;
104 break;
105 }
106 }
107 if(!inside)
108 break;
109 }
110 cell.Range() = typename BaseType::CellRange(*cur, h);
111 *cur = h;
112 return cell.Level();
113 }
114 else
115 {
116 // rebuild children
117 typename BaseType::HandleType start = *cur;
118 unsigned int numChilds = 0;
119 size_t maxDepth = 0;
120 for(unsigned int i = 0; i < CellType::NChildren; ++i)
121 {
122 if(!this->ExistChild(cell, i))
123 continue;
124 PointType cmin, cmax;
125 for(unsigned int j = 0; j < BaseType::m_dim; ++j)
126 {
127 if(i & (1 << (BaseType::m_dim - j - 1)))
128 {
129 cmin[j] = cell.Center()[j];
130 cmax[j] = max[j];
131 }
132 else
133 {
134 cmin[j] = min[j];
135 cmax[j] = cell.Center()[j];
136 }
137 }
138 size_t d = Rebuild(cell, i, cmin, cmax, cur);
139 if(d > maxDepth)
140 maxDepth = d;
141 if(cell[i].Size() == 0)
142 {
143 delete &(cell[i]);
144 cell.Child(i, (CellType *)1);
145 }
146 else
147 ++numChilds;
148 }
149 cell.Range() = typename BaseType::CellRange(start, *cur);
150 if(numChilds == 0)
151 {
152 cell.Child(0, NULL);
153 maxDepth = cell.Level();
154 }
155 else if(cell.Size() < BaseType::MaxBucketSize())
156 {
157 // make cell a leaf
158 for(unsigned int i = 0; i < CellType::NChildren; ++i)
159 {
160 if(!this->ExistChild(cell, i))
161 continue;
162 delete &(cell[i]);
163 cell.Child(i, NULL);
164 }
165 cell.Child(0, NULL);
166 maxDepth = cell.Level();
167 }
168 return maxDepth;
169 }
170 }
171 };
172 };
173
174 #endif