size_t RansacShapeDetector::Detect(PointCloud &pc, size_t beginIdx, size_t endIdx, MiscLib::Vector< std::pair< RefCountPtr< PrimitiveShape >, size_t > > *shapes) { size_t pcSize = endIdx - beginIdx; /* * Initialization part */ srand((unsigned int)time(NULL)); rn_setseed((size_t)time(NULL)); CandidatesType candidates; ScorePrimitiveShapeVisitor< FlatNormalThreshPointCompatibilityFunc, ImmediateOctreeType > subsetScoreVisitor(m_options.m_epsilon, m_options.m_normalThresh); ScorePrimitiveShapeVisitor< FlatNormalThreshPointCompatibilityFunc, IndexedOctreeType > globalScoreVisitor(3 * m_options.m_epsilon, m_options.m_normalThresh); // construct random subsets size_t subsets = std::max(int(std::floor(std::log((float)pcSize)/std::log(2.f)))-9, 2); GfxTL::AACube< GfxTL::Vector3Df > bcube; bcube.Bound(pc.begin() + beginIdx, pc.begin() + endIdx); // construct stratified subsets MiscLib::Vector< ImmediateOctreeType * > octrees(subsets); for(size_t i = octrees.size(); i;) { --i; size_t subsetSize = pcSize; if(i) { subsetSize = subsetSize >> 1; MiscLib::Vector< size_t > subsetIndices(subsetSize); size_t bucketSize = pcSize / subsetSize; for(size_t j = 0; j < subsetSize; ++j) { size_t index = rn_rand() % bucketSize; index += j * bucketSize; if(index >= pcSize) index = pcSize - 1; subsetIndices[j] = index + beginIdx; } // move all the indices to the end std::sort(subsetIndices.begin(), subsetIndices.end(), std::greater< size_t >()); for(size_t j = pcSize - 1, i = 0; i < subsetIndices.size(); --j, ++i) std::swap(pc[j + beginIdx], pc[subsetIndices[i]]); } octrees[i] = new ImmediateOctreeType; octrees[i]->ContainedData(&pc); octrees[i]->DataRange(pcSize - subsetSize + beginIdx, pcSize + beginIdx); octrees[i]->MaxBucketSize() = 20; octrees[i]->MaxSubdivisionLevel() = 10; octrees[i]->Build(bcube); pcSize -= subsetSize; } pcSize = endIdx - beginIdx; // construct one global octree MiscLib::Vector< size_t > globalOctreeIndices(pcSize); for(size_t i = 0; i < pcSize; ++i) globalOctreeIndices[i] = i + beginIdx; IndexedOctreeType globalOctree; globalOctree.MaxBucketSize() = 20; globalOctree.MaxSubdivisionLevel() = 10; globalOctree.IndexedData(globalOctreeIndices.begin(), globalOctreeIndices.end(), pc.begin()); globalOctree.Build(bcube); size_t globalOctTreeMaxNodeDepth = globalOctree.MaxDepth(); MiscLib::Vector< double > sampleLevelProbability( globalOctTreeMaxNodeDepth + 1); for(size_t i = 0; i < sampleLevelProbability.size(); ++i) sampleLevelProbability[i] = 1.0 / sampleLevelProbability.size(); size_t drawnCandidates = 0; // keep track of number of candidates // that have been generated float maxForgottenCandidate = 0; // the maximum size of a canidate // that has been forgotten size_t numTries = 0; // number of loops since last successful candidate size_t numShapes = 0; // number of shapes found since the // last housekeeping size_t numInvalid = 0; // number of points that have been assigned // to a shape since the last housekeeping MiscLib::Vector< int > shapeIndex(pc.size(), -1); // maintains for every // point the index of the shape it has been assigned to or // -1 if the point is not assigned yet subsetScoreVisitor.SetShapeIndex(shapeIndex); globalScoreVisitor.SetShapeIndex(shapeIndex); size_t currentSize = pcSize; do { MiscLib::Vector< std::pair< float, size_t > > sampleLevelScores( sampleLevelProbability.size()); for(size_t i = 0; i < sampleLevelScores.size(); ++i) sampleLevelScores[i] = std::make_pair(0.f, 0u); MiscLib::Vector< double >sampleLevelProbSum(sampleLevelProbability.size()); sampleLevelProbSum[0] = sampleLevelProbability[0]; for(size_t i = 1; i < sampleLevelProbSum.size() - 1; ++i) sampleLevelProbSum[i] = sampleLevelProbability[i] + sampleLevelProbSum[i - 1]; sampleLevelProbSum[sampleLevelProbSum.size() - 1] = 1; // generate candidates float bestExpectedValue = 0; if(candidates.size()) bestExpectedValue = candidates.back().ExpectedValue(); bestExpectedValue = std::min((float)(currentSize - numInvalid), bestExpectedValue); do { GenerateCandidates(globalOctree, octrees, pc, subsetScoreVisitor, currentSize, numInvalid, sampleLevelProbSum, &drawnCandidates, &sampleLevelScores, &bestExpectedValue, &candidates); } while(CandidateFailureProbability(bestExpectedValue, currentSize - numInvalid, drawnCandidates, globalOctTreeMaxNodeDepth) > m_options.m_probability && CandidateFailureProbability(m_options.m_minSupport, currentSize - numInvalid, drawnCandidates, globalOctTreeMaxNodeDepth) > m_options.m_probability); // find the best candidate: float bestCandidateFailureProbability; float failureProbability = std::numeric_limits< float >::infinity(); bool foundCandidate = false; size_t firstCandidateSize = 0; while(FindBestCandidate(candidates, octrees, pc, subsetScoreVisitor, currentSize, drawnCandidates, numInvalid, std::max((size_t)m_options.m_minSupport, (size_t)(0.8f * firstCandidateSize)), globalOctTreeMaxNodeDepth, &maxForgottenCandidate, &bestCandidateFailureProbability)) { if(!foundCandidate) { // this is the first candidate firstCandidateSize = (size_t)candidates.back().LowerBound(); // rescore levels for(size_t i = 0; i < sampleLevelScores.size(); ++i) sampleLevelScores[i] = std::make_pair(0.f, 0u); for(size_t i = 0; i < candidates.size(); ++i) { if(candidates[i].ExpectedValue() * 1.4f > candidates.back().ExpectedValue()) { size_t candLevel = std::min(sampleLevelScores.size() - 1, candidates[i].Level()); ++sampleLevelScores[candLevel].first; ++sampleLevelScores[candLevel].second; } } UpdateLevelWeights(0.5f, sampleLevelScores, &sampleLevelProbability); } foundCandidate = true; if(bestCandidateFailureProbability < failureProbability) failureProbability = bestCandidateFailureProbability; std::string candidateDescription; candidates.back().Shape()->Description(&candidateDescription); // do fitting if(m_options.m_fitting != Options::NO_FITTING) { candidates.back().GlobalScore(globalScoreVisitor, globalOctree); candidates.back().ConnectedComponent(pc, m_options.m_bitmapEpsilon); Candidate clone; candidates.back().Clone(&clone); float oldScore, newScore; size_t oldSize, newSize; // get the weight once newScore = clone.GlobalWeightedScore( globalScoreVisitor, globalOctree, pc, 3 * m_options.m_epsilon, m_options.m_normalThresh, m_options.m_bitmapEpsilon ); newSize = std::max(clone.Size(), candidates.back().Size()); bool allowDifferentShapes = false; size_t fittingIter = 0; do { ++fittingIter; oldScore = newScore; oldSize = newSize; std::pair< size_t, float > score; PrimitiveShape *shape; if(shape = Fit(allowDifferentShapes, *clone.Shape(), pc, clone.Indices()->begin(), clone.Indices()->end(), &score)) { clone.Shape(shape); newScore = clone.GlobalWeightedScore( globalScoreVisitor, globalOctree, pc, 3 * m_options.m_epsilon, m_options.m_normalThresh, m_options.m_bitmapEpsilon ); newSize = clone.Size(); shape->Release(); if(newScore > oldScore && newSize > m_options.m_minSupport) clone.Clone(&candidates.back()); } allowDifferentShapes = false; } while(newScore > oldScore && fittingIter < 3); } else { candidates.back().GlobalScore(globalScoreVisitor, globalOctree); candidates.back().ConnectedComponent(pc, m_options.m_bitmapEpsilon); } if(candidates.back().Size() == 0) std::cout << "ERROR: candidate size == 0 after fitting" << std::endl; // best candidate is ok! // remove the points shapes->push_back(std::make_pair(RefCountPtr< PrimitiveShape >(candidates.back().Shape()), candidates.back().Indices()->size())); for(size_t i = 0; i < candidates.back().Indices()->size(); ++i) shapeIndex[(*(candidates.back().Indices()))[i]] = numShapes; ++numShapes; // update drawn candidates to reflect removal of points // get the percentage of candidates that are invalid drawnCandidates = std::pow(1.f - (candidates.back().Indices()->size() / float(currentSize - numInvalid)), 3.f) * drawnCandidates; numInvalid += candidates.back().Indices()->size(); candidates.pop_back(); if(numInvalid > currentSize / 4) // more than half of the points assigned? { // do a housekeeping step //this is a two stage procedure: // 1) determine the new address of each Point and store it in the shapeIndex array // 2) swap Points according to new addresses size_t begin = beginIdx, end = beginIdx + currentSize; // these hold the address ranges for the lately detected shapes MiscLib::Vector< size_t > shapeIterators(numShapes); int shapeIt = shapes->size() - numShapes; for(size_t i = 0; i < numShapes; ++i, ++shapeIt) shapeIterators[i] = end -= ((*shapes)[shapeIt]).second; MiscLib::Vector< size_t > subsetSizes(octrees.size(), 0); for(size_t i = beginIdx, j = 0; i < beginIdx + currentSize; ++i) if(shapeIndex[i] < 0) { if(i >= octrees[j]->end() - pc.begin() + beginIdx && j < octrees.size() - 1) ++j; shapeIndex[i] = begin++; ++subsetSizes[j]; } else shapeIndex[i] = shapeIterators[shapeIndex[i]]++; // check if small subsets should be merged size_t mergedSubsets = 0; if(subsetSizes[0] < 500 && subsetSizes.size() > 1) { // should be merged while(subsetSizes[0] < 500 && subsetSizes.size() > 1) { subsetSizes[1] += subsetSizes[0]; subsetSizes.erase(subsetSizes.begin()); delete octrees[0]; octrees.erase(octrees.begin()); ++mergedSubsets; } } // reindex global octree size_t minInvalidIndex = currentSize - numInvalid + beginIdx; int j = 0; #ifdef DOPARALLEL #pragma omp parallel for schedule(static) #endif for(int i = 0; i < static_cast<int>(globalOctreeIndices.size()); ++i) if(shapeIndex[globalOctreeIndices[i]] < minInvalidIndex) globalOctreeIndices[j++] = shapeIndex[globalOctreeIndices[i]]; globalOctreeIndices.resize(currentSize - numInvalid); // reindex candidates (this also recomputes the bounds) #ifdef DOPARALLEL #pragma omp parallel for schedule(static) #endif for(int i = 0; i < static_cast<int>(candidates.size()); ++i) candidates[i].Reindex(shapeIndex, minInvalidIndex, mergedSubsets, subsetSizes, pc, currentSize - numInvalid, m_options.m_epsilon, m_options.m_normalThresh, m_options.m_bitmapEpsilon); //regarding swapping it is best to swap both the addresses and the data for(size_t i = beginIdx; i < beginIdx + currentSize; ++i) while(i != shapeIndex[i]) { pc.swapPoints(i, shapeIndex[i]); std::swap(shapeIndex[i], shapeIndex[shapeIndex[i]]); } numInvalid = 0; // rebuild subset octrees if(mergedSubsets) // the octree for the first subset has to be constructed { //std::cout << "Attention: Merged " << mergedSubsets << " subsets!" << std::endl; MiscLib::Vector< size_t > shuffleIndices(beginIdx + subsetSizes[0]), reindex(beginIdx + subsetSizes[0]); for(size_t i = 0; i < shuffleIndices.size(); ++i) shuffleIndices[i] = i; delete octrees[0]; octrees[0] = new ImmediateOctreeType(); octrees[0]->ContainedData(&pc); octrees[0]->DataRange(beginIdx, beginIdx + subsetSizes[0]); octrees[0]->MaxBucketSize() = 20; octrees[0]->MaxSubdivisionLevel() = 10; octrees[0]->ShuffleIndices(&shuffleIndices); octrees[0]->Build(bcube); octrees[0]->ShuffleIndices(NULL); for(size_t i = 0; i < shuffleIndices.size(); ++i) reindex[shuffleIndices[i]] = i; // reindex global octree #ifdef DOPARALLEL #pragma omp parallel for schedule(static) #endif for(int i = 0; i < static_cast<int>(globalOctreeIndices.size()); ++i) if(globalOctreeIndices[i] < reindex.size()) globalOctreeIndices[i] = reindex[globalOctreeIndices[i]]; // reindex candidates #ifdef DOPARALLEL #pragma omp parallel for schedule(static, 100) #endif for(int i = 0; i < static_cast<int>(candidates.size()); ++i) candidates[i].Reindex(reindex); for(size_t i = 1, begin = subsetSizes[0] + beginIdx; i < octrees.size(); begin += subsetSizes[i], ++i) { octrees[i]->DataRange(begin, begin + subsetSizes[i]); octrees[i]->Rebuild(); if(octrees[i]->Root()->Size() != subsetSizes[i]) std::cout << "ERROR IN REBUILD!!!!" << std::endl; } } else for(size_t i = 0, begin = beginIdx; i < octrees.size(); begin += subsetSizes[i], ++i) { octrees[i]->DataRange(begin, begin + subsetSizes[i]); octrees[i]->Rebuild(); } //so everything is in its correct place, but we need to update the global octree ranges currentSize = globalOctreeIndices.size(); globalOctree.IndexedRange(globalOctreeIndices.begin(), globalOctreeIndices.end()); globalOctTreeMaxNodeDepth = globalOctree.Rebuild(); if(globalOctree.Root()->Size() != globalOctreeIndices.size()) std::cout << "ERROR IN GLOBAL REBUILD!" << std::endl; sampleLevelProbability.resize(globalOctTreeMaxNodeDepth + 1); //shapeIndex.resize(globalOctreeIndices.size()); std::fill(shapeIndex.begin() + beginIdx, shapeIndex.begin() + beginIdx + currentSize, -1); numShapes = 0; } else { // the bounds of the candidates have become invalid and have to be // recomputed #ifdef DOPARALLEL #pragma omp parallel for schedule(static, 100) #endif for(int i = 0; i < static_cast<int>(candidates.size()); ++i) candidates[i].RecomputeBounds(octrees, pc, subsetScoreVisitor, currentSize - numInvalid, m_options.m_epsilon, m_options.m_normalThresh, m_options.m_bitmapEpsilon); } // remove all candidates that have become obsolete std::sort(candidates.begin(), candidates.end(), std::greater< Candidate >()); size_t remainingCandidates = 0; for(size_t i = 0; i < candidates.size(); ++i) if(candidates[i].ExpectedValue() >= m_options.m_minSupport && candidates[i].Size() > 0) candidates[remainingCandidates++] = candidates[i]; candidates.resize(remainingCandidates); } // Ende abgrasen if(foundCandidate) { std::sort(candidates.begin(), candidates.end(), std::greater< Candidate >()); size_t remainingCandidates = 0; size_t nonConnectedCount = 0; for(size_t i = 0; i < candidates.size(); ++i) if(candidates[i].ExpectedValue() >= m_options.m_minSupport && (candidates[i].ComputedSubsets() > octrees.size() - 3 || nonConnectedCount++ < 500)) candidates[remainingCandidates++] = candidates[i]; else if(candidates[i].ExpectedValue() > maxForgottenCandidate) maxForgottenCandidate = candidates[i].ExpectedValue(); candidates.resize(remainingCandidates); numTries = 0; } else { numTries++; } } while(CandidateFailureProbability(m_options.m_minSupport, currentSize - numInvalid, drawnCandidates, globalOctTreeMaxNodeDepth) > m_options.m_probability && (currentSize - numInvalid) >= m_options.m_minSupport); if(numInvalid) { // rearrange the last shapes //this is a two stage procedure: // 1) determine the new address of each Point and store it in the shapeIndex array // 2) swap Points according to new addresses size_t begin = beginIdx, end = beginIdx + currentSize; // these hold the address ranges for the lately detected shapes MiscLib::Vector< size_t > shapeIterators(numShapes); int shapeIt = shapes->size() - numShapes; for(size_t i = 0; i < numShapes; ++i, ++shapeIt) shapeIterators[i] = end -= ((*shapes)[shapeIt]).second; for(size_t i = beginIdx; i < beginIdx + currentSize; ++i) if(shapeIndex[i] < 0) shapeIndex[i] = begin++; else shapeIndex[i] = shapeIterators[shapeIndex[i]]++; //regarding swapping it is best to swap both the addresses and the data for(size_t i = beginIdx; i < beginIdx + currentSize; ++i) while(i != shapeIndex[i]) { pc.swapPoints(i, shapeIndex[i]); std::swap(shapeIndex[i], shapeIndex[shapeIndex[i]]); } } // clean up subset octrees for(size_t i = 0; i < octrees.size(); ++i) delete octrees[i]; // optimize parametrizations size_t eidx = endIdx; for(size_t i = 0; i < shapes->size(); ++i) { size_t bidx = eidx - (*shapes)[i].second; (*shapes)[i].first->OptimizeParametrization(pc, bidx, eidx, m_options.m_bitmapEpsilon); eidx = bidx; } // prune nonsense shapes for(size_t i = shapes->size(); i != 0; --i) { if(shapes->at(i - 1).second == 0) shapes->erase(shapes->begin() + i - 1); } return currentSize - numInvalid; }