bool CurvatureFeatureExtractor::computeOccludedObject(const core::Img32f &depthImg, core::DataSegment<float,4> &xyz, core::DataSegment<float, 4> &normals, SurfaceFeatureExtractor::SurfaceFeature feature1, SurfaceFeatureExtractor::SurfaceFeature feature2, std::vector<int> &surface1, std::vector<int> &surface2, int w, float maxError, int ransacPasses, float distanceTolerance, float outlierTolerance){ //select most populated bin (same bin for both histograms) float maxBinValue=0; utils::Point maxBin(0,0); for(int y=0; y<feature1.normalHistogram.getSize().height; y++){ for(int x=0; x<feature1.normalHistogram.getSize().width; x++){ float binValue = std::min(feature1.normalHistogramChannel(x,y), feature2.normalHistogramChannel(x,y)); if(binValue>maxBinValue){ maxBinValue=binValue; maxBin.x=x; maxBin.y=y; } } } //backproject the points std::vector<int> pointIDs1 = backprojectPointIDs(normals, maxBin, surface1); std::vector<int> pointIDs2 = backprojectPointIDs(normals, maxBin, surface2); std::vector<Vec> points1 = createPointsFromIDs(xyz, pointIDs1); std::vector<Vec> points2 = createPointsFromIDs(xyz, pointIDs2); //fit line with RANSAC (faster than linear regression) float minError = 100000; std::pair<utils::Point,utils::Point> pointPairImg; for(int i=0; i<ransacPasses; i++){ float currentError=0; std::pair<Vec,Vec> currentPointPair; std::pair<int,int> currentPointPairID; currentPointPairID.first = pointIDs1[rand()%pointIDs1.size()]; currentPointPairID.second = pointIDs2[rand()%pointIDs2.size()]; currentPointPair.first = xyz[currentPointPairID.first]; currentPointPair.second = xyz[currentPointPairID.second]; for(unsigned int i=0; i<points1.size(); i++){ currentError+=linePointDistance(currentPointPair, points1[i]); } for(unsigned int i=0; i<points2.size(); i++){ currentError+=linePointDistance(currentPointPair, points2[i]); } currentError/=points1.size()+points2.size(); if(currentError<minError){ minError=currentError; pointPairImg.first=idToPoint(currentPointPairID.first,w); pointPairImg.second=idToPoint(currentPointPairID.second,w); } } //occlusion check if(minError<maxError){ return SegmenterUtils::occlusionCheck((core::Img32f&)depthImg, pointPairImg.first, pointPairImg.second, distanceTolerance, outlierTolerance); } return false; }
Chunk& filterLows(int preserveCount = 1) { std::vector<std::pair<size_t, Complex>> stash; for (int i = 0; i < preserveCount; ++i) { size_t index = maxBin(); stash.emplace_back(index, signal[index]); signal[index].real(0); signal[index].imag(0); } for (size_t i = 0; i < signal.size(); ++i) { signal[i].real(0); signal[i].imag(0); } for (int i = 0; i < preserveCount; ++i) { signal[stash[i].first] = stash[i].second; } return *this; }
float maxVal() const { return signal[maxBin()].real(); }
float frequency(size_t samplingRate = 44100) const { return binFrequency(maxBin(), samplingRate); }