Debian Codegraph @

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

raw
1 #ifndef CANDIDATE_HEADER
2 #define CANDIDATE_HEADER
3 #include "ScoreComputer.h"
4 #include <MiscLib/RefCountPtr.h>
5 #include "PrimitiveShape.h"
6 #include <MiscLib/Vector.h>
7 #include <limits>
8 #include <MiscLib/RefCounted.h>
9 #include <MiscLib/Random.h>
10 #include <MiscLib/NoShrinkVector.h>
11 #include "Octree.h"
12 #include <algorithm>
13
14 #ifndef DLL_LINKAGE
15 #define DLL_LINKAGE
16 #endif
17
18 class DLL_LINKAGE Candidate
19 {
20 public:
21 Candidate();
22 Candidate(PrimitiveShape *shape, size_t level);
23 PrimitiveShape *Shape() { return m_shape; }
24 void Shape(PrimitiveShape *shape) { m_shape = shape; }
25 float LowerBound() const { return m_lowerBound; }
26 float UpperBound() const { return m_upperBound; }
27 MiscLib::RefCounted< MiscLib::Vector< size_t > > *Indices() { return m_indices; }
28 void Indices(MiscLib::RefCounted< MiscLib::Vector< size_t > > *indices) { m_indices = indices; }
29 size_t ComputedSubsets() const { return m_subset; }
30 float ExpectedValue() const { return (m_lowerBound + m_upperBound) / 2.f; }
31 void SetSubset(size_t subset) { m_subset = subset; }
32 size_t Level() const { return m_level; }
33 void Reset();
34 template< class ScoreVisitorT >
35 bool ImproveBounds(const MiscLib::Vector< ImmediateOctreeType * > &octrees,
36 const PointCloud &pc, ScoreVisitorT &scoreVisitor,
37 size_t currentSize, float bitmapEpsilon,
38 size_t maxSubset, size_t minPoints = 500);
39 template< class ScoreVisitorT >
40 void RecomputeBounds(const MiscLib::Vector< ImmediateOctreeType * > &octrees,
41 const PointCloud &pc, ScoreVisitorT &scoreVisitor, size_t currentSize,
42 float epsilon, float normalThresh, float bitmapEpsilon);
43 void Reindex(const MiscLib::Vector< int > &newIndices, int minInvalidIndex,
44 size_t mergedSubsets, const MiscLib::Vector< size_t > &subsetSizes,
45 const PointCloud &pc, size_t currentSize, float epsilon,
46 float normalThresh, float bitmapEpsilon);
47 void Reindex(const MiscLib::Vector< size_t > &reindex);
48 template< class ScoreVisitorT >
49 void GlobalScore(ScoreVisitorT &scoreVisitor,
50 const IndexedOctreeType &oct);
51 float WeightedScore(const PointCloud &pc, float epsilon,
52 float normalThresh) const;
53 void ConnectedComponent(const PointCloud &pc, float bitmapEpsilon, float* borderRatio = 0 );
54 template< class ScoreVisitorT >
55 float GlobalWeightedScore( ScoreVisitorT &scoreVisitor, const IndexedOctreeType &oct,
56 const PointCloud &pc, float epsilon, float normalThresh,
57 float bitmapEpsilon );
58 inline bool operator<(const Candidate &c) const;
59 inline bool operator>(const Candidate &c) const;
60 inline bool operator<=(const Candidate &c) const;
61 inline bool operator>=(const Candidate &c) const;
62 inline void Clone(Candidate *c) const;
63 bool IsEquivalent(const Candidate &c, const PointCloud &pc,
64 float epsilon, float normalThresh) const;
65 size_t Size() const { return m_indices->size(); }
66
67 private:
68 inline void GetBounds(size_t sampledPoints, size_t totalPoints);
69 inline void Induct(double totalSize, double sampleSize,
70 double totalCorrectSize, float *lower, float *upper) const;
71 inline void Deduct(double totalSize, double sampleSize,
72 double totalCorrectSize, float *lower, float *upper) const;
73
74 void GetScore( const PointCloud& pc, float bitmapEpsilon, bool doFiltering );
75 void GetScoreMaxCCSize( const PointCloud& pc, float bitmapEpsilon, bool doFiltering );
76 void GetScoreMaxCCMinBorder( const PointCloud& pc, float bitmapEpsilon, bool doFiltering );
77
78 float GetVariance( const PointCloud &pc );
79 float GetPseudoVariance( const PointCloud &pc );
80
81 private:
82 MiscLib::RefCountPtr< PrimitiveShape > m_shape;
83 size_t m_subset;
84 float m_lowerBound;
85 float m_upperBound;
86 MiscLib::RefCountPtr< MiscLib::RefCounted< MiscLib::Vector< size_t > > > m_indices;
87 size_t m_level;
88 bool m_hasConnectedComponent;
89 size_t m_score;
90 };
91
92 bool Candidate::operator<(const Candidate &c) const
93 {
94 return ExpectedValue() < c.ExpectedValue();
95 }
96
97 bool Candidate::operator>(const Candidate &c) const
98 {
99 return ExpectedValue() > c.ExpectedValue();
100 }
101
102 bool Candidate::operator<=(const Candidate &c) const
103 {
104 return ExpectedValue() <= c.ExpectedValue();
105 }
106
107 bool Candidate::operator>=(const Candidate &c) const
108 {
109 return ExpectedValue() >= c.ExpectedValue();
110 }
111
112 void Candidate::Clone(Candidate *c) const
113 {
114 c->m_shape = m_shape->Clone();
115 c->m_shape->Release();
116 c->m_subset = m_subset;
117 c->m_lowerBound = m_lowerBound;
118 c->m_upperBound = m_upperBound;
119 c->m_indices = new MiscLib::RefCounted< MiscLib::Vector< size_t > >(*m_indices);
120 c->m_indices->Release();
121 c->m_level = m_level;
122 c->m_hasConnectedComponent = m_hasConnectedComponent;
123 c->m_score = m_score;
124 }
125
126 void Candidate::GetBounds(size_t sampledPoints, size_t totalPoints)
127 {
128 Induct((double)totalPoints, (double)sampledPoints, (double)m_score, &m_lowerBound,
129 &m_upperBound);
130 m_lowerBound = std::max(0.f, m_lowerBound);
131 }
132
133 void Candidate::Induct(double totalSize, double sampleSize,
134 double totalCorrectSize, float *lower, float *upper) const
135 {
136 Deduct(-2.0 - sampleSize, -2.0 - totalSize, -1.0 - totalCorrectSize,
137 lower, upper);
138 *lower = -1.f - *lower;
139 *upper = -1.f - *upper;
140 }
141
142 void Candidate::Deduct(double totalSize, double sampleSize,
143 double totalCorrectSize, float *lower, float *upper) const
144 {
145 double nI = sampleSize * totalCorrectSize;
146 double denom = nI * (totalSize - sampleSize) *
147 (totalSize - totalCorrectSize);
148 double frac = denom / (totalSize - 1);
149 double dev = std::sqrt(frac);
150 *lower = float((nI - dev) / totalSize); // 95% quantile
151 *upper = float((nI + dev) / totalSize); // 95% quantile
152 }
153
154 template< class ScoreVisitorT >
155 bool Candidate::ImproveBounds(const MiscLib::Vector< ImmediateOctreeType * > &octrees,
156 const PointCloud &pc, ScoreVisitorT &scoreVisitor,
157 size_t currentSize, float bitmapEpsilon,
158 size_t maxSubset, size_t minPoints)
159 {
160 if(m_subset >= maxSubset)
161 return false;
162 if(m_subset >= octrees.size())
163 return false;
164
165 size_t sampledPoints = 0;
166 for(size_t i = 0; i < m_subset; ++i)
167 sampledPoints += octrees[i]->size();
168
169 size_t newlySampledPoints = 0;
170 scoreVisitor.SetIndices(m_indices);
171 do
172 {
173 scoreVisitor.SetOctree(*octrees[m_subset]);
174 m_shape->Visit(&scoreVisitor);
175 newlySampledPoints += octrees[m_subset]->size();
176 sampledPoints += octrees[m_subset]->size();
177 ++m_subset;
178 }
179 while(m_subset < octrees.size() && newlySampledPoints < minPoints);
180
181 m_score = m_indices->size();
182 GetBounds(sampledPoints, currentSize);
183
184 // check if connected component is worthwhile
185 if( m_subset == 1)
186 {
187 return true;
188 }
189 if( m_hasConnectedComponent ||
190 (2.f * ( m_upperBound - ( m_lowerBound / .7f)) /
191 (m_upperBound + (m_lowerBound / .7f))) < .3f)
192 {
193 if(!m_hasConnectedComponent && m_indices->size() < 2)
194 {
195 return true;
196 }
197 m_hasConnectedComponent = true;
198 m_score = m_shape->ConnectedComponent(pc,
199 (4 << ((octrees.size() - m_subset) / 2)) * bitmapEpsilon,
200 m_indices, false);
201 m_indices->resize(m_score);
202 if(m_subset >= octrees.size())
203 {
204 GetScore( pc, bitmapEpsilon, true);
205 m_upperBound = m_lowerBound = static_cast<float>(m_score);
206 return true;
207 }
208 GetScore( pc, (2 << ((octrees.size() - m_subset) / 2)) * bitmapEpsilon, false);
209
210 GetBounds( sampledPoints, currentSize);
211 }
212 return true;
213 }
214
215 template< class ScoreVisitorT >
216 void Candidate::RecomputeBounds(const MiscLib::Vector< ImmediateOctreeType * > &octrees,
217 const PointCloud &pc, ScoreVisitorT &scoreVisitor, size_t currentSize,
218 float epsilon, float normalThresh, float bitmapEpsilon)
219 {
220 // run over indices and check if still unassigned
221 const MiscLib::Vector< int > &shapeIndex = scoreVisitor.GetShapeIndex();
222 size_t indicesSize = m_indices->size();
223 for(size_t i = 0; i < indicesSize;)
224 {
225 if(shapeIndex[(*m_indices)[i]] != -1)
226 {
227 --indicesSize;
228 std::swap((*m_indices)[i], (*m_indices)[indicesSize]);
229 }
230 else
231 ++i;
232 }
233
234 m_score = indicesSize;
235
236 if(m_hasConnectedComponent)
237 {
238 if(m_indices->size() != indicesSize) // dirty!
239 {
240 if((float)indicesSize / m_indices->size() > 0.95)
241 {
242 m_indices->resize(indicesSize);
243
244 GetScore( pc, m_subset >= octrees.size()? bitmapEpsilon :
245 (4 << ((octrees.size() - m_subset) / 2)) * bitmapEpsilon,
246 m_subset >= octrees.size());
247
248 if(m_subset >= octrees.size())
249 {
250 m_upperBound = m_lowerBound = (float)m_indices->size();
251 return;
252 }
253 // reevaluate bounds
254 }
255 else
256 {
257 m_subset = 0;
258 m_hasConnectedComponent = false;
259 m_indices->clear();
260 m_score = 0;
261 ImproveBounds(octrees, pc, scoreVisitor,
262 currentSize, bitmapEpsilon, 1);
263 return;
264 }
265 }
266 else if(m_subset > octrees.size())
267 return;
268 // reevaluate bounds
269 }
270 else
271 {
272 m_indices->resize(indicesSize);
273 m_score = m_indices->size();
274 }
275 size_t sampledPoints = 0,
276 endi = std::min(m_subset, octrees.size());
277 for(size_t i = 0; i < endi; ++i)
278 sampledPoints += octrees[i]->size();
279
280 GetBounds(sampledPoints, currentSize);
281 }
282
283 template< class ScoreVisitorT >
284 void Candidate::GlobalScore(ScoreVisitorT &scoreVisitor,
285 const IndexedOctreeType &oct)
286 {
287 m_indices->clear();
288 scoreVisitor.SetOctree(oct);
289 scoreVisitor.SetIndices(m_indices);
290 m_shape->Visit(&scoreVisitor);
291 m_lowerBound = m_upperBound = (float)m_indices->size();
292 }
293
294 template< class ScoreVisitorT >
295 float Candidate::GlobalWeightedScore( ScoreVisitorT &scoreVisitor, const IndexedOctreeType &oct,
296 const PointCloud &pc, float epsilon, float normalThresh,
297 float bitmapEpsilon )
298 {
299 GlobalScore( scoreVisitor, oct );
300 ConnectedComponent( pc, bitmapEpsilon );
301 return WeightedScore( pc, epsilon, normalThresh );
302 }
303
304 #endif