bool RansacShapeDetector::FindBestCandidate(CandidatesType &candidates,
const MiscLib::Vector< ImmediateOctreeType * > &octrees, const PointCloud &pc,
ScoreVisitorT &scoreVisitor, size_t currentSize,
size_t drawnCandidates, size_t numInvalid, size_t minSize, float numLevels,
float *maxForgottenCandidate, float *candidateFailProb) const
{
if(!candidates.size())
return false;
size_t maxImproveSubsetDuringMaxSearch = octrees.size();
// sort by expected value
std::sort(candidates.begin(), candidates.end());
// check if max is smaller than forgotten candidate
if(candidates.size() && candidates.back().ExpectedValue() < *maxForgottenCandidate)
{
// drawn candidates is wrong!
// need to correct the value
drawnCandidates = std::max(candidates.size(), (size_t)1);
*maxForgottenCandidate = 0;
}
MiscLib::Vector< Candidate * > candHeap;
for(size_t i = candidates.size() - 1; i != -1; --i)
{
if(CandidateFailureProbability(
candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels) > m_options.m_probability)
break;
candHeap.push_back(&candidates[i]);
}
if(!candHeap.size())
{
return false;
}
std::make_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
MiscLib::Vector< Candidate * > beatenCands;
Candidate *trial = candHeap.front();
std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
candHeap.pop_back();
float bestCandidateFailureProbability;
while(candHeap.size())
{
if(trial->IsEquivalent(*candHeap.front(), pc, m_options.m_epsilon,
m_options.m_normalThresh))
{
std::pop_heap(candHeap.begin(), candHeap.end(),
CandidateHeapPred());
candHeap.pop_back();
continue;
}
bool isEquivalent = false;
for(size_t j = 0; j < beatenCands.size(); ++j)
{
if(beatenCands[j]->IsEquivalent(*candHeap.front(), pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
isEquivalent = true;
break;
}
}
if(isEquivalent)
{
std::pop_heap(candHeap.begin(), candHeap.end(),
CandidateHeapPred());
candHeap.pop_back();
continue;
}
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
while((bestCandidateFailureProbability <= m_options.m_probability)
&& (*trial >= *candHeap.front())
&& (trial->UpperBound() >= minSize)
&& trial->ImproveBounds(octrees, pc, scoreVisitor, currentSize,
m_options.m_bitmapEpsilon, octrees.size()))
{
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize
&& trial->ComputedSubsets() >= octrees.size()
&& *trial >= *candHeap.front())
break;
if(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize)
{
candHeap.push_back(trial);
std::push_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
}
else if((int)trial->ComputedSubsets()
> std::max(2, ((int)octrees.size()) - 2))
beatenCands.push_back(trial);
//nextCandidate
trial = candHeap.front();
std::pop_heap(candHeap.begin(), candHeap.end(), CandidateHeapPred());
candHeap.pop_back();
}
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
while(bestCandidateFailureProbability <= m_options.m_probability
&& trial->UpperBound() >= minSize
&& trial->ImproveBounds(octrees, pc, scoreVisitor, currentSize,
m_options.m_bitmapEpsilon, octrees.size()))
{
bestCandidateFailureProbability = CandidateFailureProbability(
trial->ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if((bestCandidateFailureProbability > m_options.m_probability
|| trial->UpperBound() < minSize)
&& (!m_autoAcceptSize || trial->UpperBound() < m_autoAcceptSize))
{
return false;
}
std::sort(candidates.begin(), candidates.end());
size_t bestCandidate = candidates.size() - 1;
bestCandidateFailureProbability = CandidateFailureProbability(
candidates.back().ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
for(size_t i = bestCandidate - 1; i != -1; --i)
{
float iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(iFailProb > m_options.m_probability || candidates[i].UpperBound() < minSize
|| candidates[i].UpperBound() < candidates[bestCandidate].LowerBound())
break;
// check if this is an identical candidate
if(candidates[bestCandidate].IsEquivalent(candidates[i], pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
continue;
}
bool isEquivalent = false;
for(size_t j = 0; j < beatenCands.size(); ++j)
{
if(beatenCands[j]->IsEquivalent(candidates[i], pc,
m_options.m_epsilon, m_options.m_normalThresh))
{
isEquivalent = true;
break;
}
}
if(isEquivalent)
{
continue;
}
do
{
if(candidates[i].UpperBound() > candidates[bestCandidate].UpperBound()
&& candidates[i].LowerBound() < candidates[bestCandidate].LowerBound())
{
bool dontBreak = candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
else if(candidates[bestCandidate].UpperBound() > candidates[i].UpperBound()
&& candidates[bestCandidate].LowerBound() < candidates[i].LowerBound())
{
bool dontBreak = candidates[bestCandidate].ImproveBounds(octrees, pc,
scoreVisitor, currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
else
{
bool dontBreak = candidates[bestCandidate].ImproveBounds(octrees, pc,
scoreVisitor, currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch);
dontBreak = candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon,
maxImproveSubsetDuringMaxSearch)
|| dontBreak;
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if(!dontBreak)
break;
}
}
while(bestCandidateFailureProbability <= m_options.m_probability
&& iFailProb <= m_options.m_probability
&& candidates[i].UpperBound() >= minSize
&& candidates[bestCandidate].UpperBound() >= minSize
&& candidates[i].UpperBound() > candidates[bestCandidate].LowerBound()
&& candidates[i].LowerBound() < candidates[bestCandidate].UpperBound()
);
if((
candidates[i] > candidates[bestCandidate]
|| bestCandidateFailureProbability > m_options.m_probability
|| candidates[bestCandidate].UpperBound() < minSize)
&& (iFailProb <= m_options.m_probability && candidates[i].UpperBound() >= minSize))
{
while(iFailProb <= m_options.m_probability && candidates[i].UpperBound() >= minSize
&& candidates[i] > candidates[bestCandidate]
&& candidates[i].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon, octrees.size()))
{
iFailProb = CandidateFailureProbability(candidates[i].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
}
if(candidates[i] > candidates[bestCandidate])
{
beatenCands.push_back(&candidates[bestCandidate]);
bestCandidate = i;
bestCandidateFailureProbability = iFailProb;
}
else
beatenCands.push_back(&candidates[i]);
}
else
beatenCands.push_back(&candidates[i]);
if(bestCandidateFailureProbability > m_options.m_probability
|| candidates[bestCandidate].UpperBound() < minSize)
break;
} // end for
while(candidates[bestCandidate].ImproveBounds(octrees, pc, scoreVisitor,
currentSize, m_options.m_bitmapEpsilon, octrees.size()));
bestCandidateFailureProbability = CandidateFailureProbability(
candidates[bestCandidate].ExpectedValue(),
currentSize - numInvalid, drawnCandidates, numLevels);
if((bestCandidateFailureProbability <= m_options.m_probability
&& candidates[bestCandidate].UpperBound() >= minSize)
|| (m_autoAcceptSize && candidates[bestCandidate].UpperBound() >= m_autoAcceptSize))
{
std::swap(candidates.back(), candidates[bestCandidate]);
*candidateFailProb = bestCandidateFailureProbability;
return true;
}
std::sort(candidates.begin(), candidates.end()/*, std::greater< Candidate >()*/);
return false;
}
