Debian Codegraph @

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

raw
1 #include "Candidate.h"
2
3 #if !defined(_WIN32) && !defined(WIN32)
4 #include <unistd.h>
5 #endif
6
7 Candidate::Candidate()
8 : m_subset(0)
9 , m_lowerBound(0)
10 , m_upperBound(0)
11 , m_level(0)
12 , m_hasConnectedComponent(false)
13 {}
14
15 Candidate::Candidate(PrimitiveShape *shape, size_t level)
16 : m_shape(shape)
17 , m_subset(0)
18 , m_lowerBound(0)
19 , m_upperBound(0)
20 , m_level(level)
21 , m_hasConnectedComponent(false)
22 {}
23
24 void Candidate::Reset()
25 {
26 m_indices->clear();
27 m_subset = 0;
28 m_lowerBound = 0;
29 m_upperBound = 0;
30 m_hasConnectedComponent = false;
31 m_score = 0;
32 }
33
34 void Candidate::Reindex(const MiscLib::Vector< int > &newIndices, int minInvalidIndex,
35 size_t mergedSubsets, const MiscLib::Vector< size_t > &subsetSizes,
36 const PointCloud &pc, size_t currentSize, float epsilon, float normalThresh,
37 float bitmapEpsilon)
38 {
39 size_t i = 0, j = 0;
40 for(; i < m_indices->size(); ++i)
41 if(newIndices[(*m_indices)[i]] < minInvalidIndex)
42 (*m_indices)[j++] = newIndices[(*m_indices)[i]];
43 if(m_subset <= mergedSubsets)
44 {
45 m_hasConnectedComponent = false;
46 m_subset = 0;
47 m_indices->clear();
48 m_lowerBound = 0;
49 m_upperBound = 0; // forget this candidate
50 m_score = 0;
51 return;
52 }
53 else
54 {
55 m_indices->resize(j);
56 m_subset -= mergedSubsets;
57 if(m_subset >= subsetSizes.size())
58 // do connected component if all subsets have been computed
59 ConnectedComponent(pc, bitmapEpsilon);
60 }
61 size_t sampledPoints = 0,
62 endi = std::min(m_subset, subsetSizes.size());;
63 for(i = 0; i < endi; ++i)
64 sampledPoints += subsetSizes[i];
65
66 GetBounds(sampledPoints, currentSize);
67 }
68
69 void Candidate::Reindex(const MiscLib::Vector< size_t > &reindex)
70 {
71 size_t reindexSize = reindex.size();
72 for(size_t i = 0; i < m_indices->size(); ++i)
73 if(m_indices->at(i) < reindexSize)
74 m_indices->at(i) = reindex[m_indices->at(i)];
75 }
76
77 float Candidate::WeightedScore(const PointCloud &pc, float epsilon,
78 float normalThresh) const
79 {
80 float score = 0;
81 #ifdef DOPARALLEL
82 #pragma omp parallel for schedule(static) reduction(+:score)
83 #endif
84 for(intptr_t i = 0; i < (intptr_t)m_indices->size(); ++i)
85 score += weigh(m_shape->Distance(pc[(*m_indices)[i]].pos), epsilon);
86 return score;
87 }
88
89 void Candidate::ConnectedComponent(const PointCloud &pc, float bitmapEpsilon, float* borderRatio )
90 {
91 size_t connectedSize = m_shape->ConnectedComponent(pc, bitmapEpsilon, m_indices, true, borderRatio);
92 m_indices->resize(connectedSize);
93 m_lowerBound = m_upperBound = (float)m_indices->size();
94 }
95
96 bool Candidate::IsEquivalent(const Candidate &c, const PointCloud &pc,
97 float epsilon, float normalThresh) const
98 {
99 if(m_shape->Identifier() != c.m_shape->Identifier())
100 return false;
101 size_t correct = 0;
102 size_t size = std::min(m_indices->size(), (size_t)9);
103 for(size_t i = 0; i < size; ++i)
104 {
105 std::pair< float, float > dn;
106 size_t idx = MiscLib::rn_rand() % m_indices->size();
107 c.m_shape->DistanceAndNormalDeviation(
108 pc[m_indices->at(idx)].pos,
109 pc[m_indices->at(idx)].normal, &dn);
110 if(dn.first < epsilon && fabs(dn.second) > normalThresh)
111 ++correct;
112 }
113 size_t tested = size;
114 size = std::min(c.m_indices->size(), (size_t)9);
115 for(size_t i = 0; i < size; ++i)
116 {
117 std::pair< float, float > dn;
118 size_t idx = MiscLib::rn_rand() % c.m_indices->size();
119 m_shape->DistanceAndNormalDeviation(
120 pc[c.m_indices->at(idx)].pos,
121 pc[c.m_indices->at(idx)].normal, &dn);
122 if(dn.first < epsilon && fabs(dn.second) > normalThresh)
123 ++correct;
124 }
125 tested += size;
126 return correct >= 2 * tested / 3;
127 }
128
129 float Candidate::GetVariance( const PointCloud &pc )
130 {
131 float variance = 0.0f;
132 float dev;
133
134 if( m_indices->size() > 0 )
135 {
136 // first pass - get expectancy
137 float expectancy = 0.0f;
138 for( int i = 0; i < m_indices->size(); ++i )
139 expectancy += fabs( m_shape->NormalDeviation( pc[(*m_indices)[i]].pos, pc[(*m_indices)[i]].normal));
140
141 expectancy /= static_cast<float>( m_indices->size());
142
143 // second pass - get real variance
144 for( int i = 0; i < m_indices->size(); ++i )
145 {
146 dev = fabs( m_shape->NormalDeviation( pc[(*m_indices)[i]].pos, pc[(*m_indices)[i]].normal)) - expectancy;
147 variance += dev * dev;
148 }
149
150 variance /= static_cast<float>( m_indices->size());
151 }
152 else
153 variance = 1.0f;
154
155 return variance;
156 }
157
158 float Candidate::GetPseudoVariance( const PointCloud &pc )
159 {
160 float variance = 0.0f;
161 float dev;
162
163 for( int i = 0; i < m_indices->size(); ++i )
164 {
165 dev = fabs( m_shape->NormalDeviation( pc[(*m_indices)[i]].pos, pc[(*m_indices)[i]].normal)) - 1.0f;
166 variance += dev * dev;
167 }
168 variance /= static_cast<float>( m_indices->size());
169
170 return variance;
171 }
172
173 void Candidate::GetScore( const PointCloud& pc, float bitmapEpsilon, bool doFiltering )
174 {
175 GetScoreMaxCCSize( pc, bitmapEpsilon, doFiltering );
176 }
177
178 void Candidate::GetScoreMaxCCSize( const PointCloud& pc, float bitmapEpsilon, bool doFiltering )
179 {
180 size_t connectedSize = m_shape->ConnectedComponent(pc, bitmapEpsilon,
181 m_indices, doFiltering );
182 m_indices->resize(connectedSize);
183
184 m_score = connectedSize;
185 }
186
187 void Candidate::GetScoreMaxCCMinBorder( const PointCloud& pc, float bitmapEpsilon, bool doFiltering )
188 {
189 float borderRatio;
190 size_t connectedSize = m_shape->ConnectedComponent(pc, bitmapEpsilon,
191 m_indices, doFiltering, &borderRatio);
192 m_indices->resize(connectedSize);
193
194 m_score = connectedSize * size_t(( 1.0f - borderRatio ) * ( 1.0f - GetVariance( pc )));
195 }