| 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 |
} |