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