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