void DepthProcessor::findBlobs() { if (!getDepthData()) return; Surface8u to8(mDepthSource->getDepthImage()); cv::Mat input( toOcv( to8)); cv::Mat gray; cv::Mat thresh; cv::cvtColor( input, gray, CV_RGB2GRAY ); cv::threshold(gray, gray, mDepthLowPass, 0, CV_THRESH_TOZERO_INV); if (mInitInitial < mInitFrames) { if (mInitInitial == 0) mInitial = gray.clone(); else mInitial = cv::max(gray, mInitial); mInitInitial++; return; } gray -= mInitial; cv::accumulateWeighted(gray, mLastGray, 0.9); mLastGray.convertTo(gray, CV_8U); cv::threshold( gray, thresh, mStepFrom, 255, CV_THRESH_BINARY ); auto& blobs = mBlobs[1 - mIndexFG]; blobs.clear(); float largest = mAreaThreshold; mContourSurfaces.pushFront(thresh.clone()); ContourVector vec; cv::findContours( thresh, vec, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE ); for( ContourVector::iterator iter = vec.begin(); iter != vec.end(); ++iter ) { float a = cv::contourArea(*iter); if (a > largest) { Blob b; b.mContourArea = a; b.mContourPoints.resize(iter->size()); copy(iter->begin(), iter->end(), b.mContourPoints.begin()); blobs.push_back(b); push_heap(blobs.begin(), blobs.end(), SortDescendingArea()); if (blobs.size() > DepthProcessor::smMAX_BLOBS) { blobs.erase(blobs.end()-1); largest = blobs.rbegin()->mContourArea; } } } for (BlobVector::iterator i = blobs.begin(); i != blobs.end(); i++) { i->mCentroid.x = i->mCentroid.y = 0.0f; float mag = 10000.0f; i->mLeftMost.x = mag; i->mRightMost.x = -mag; i->mTopMost.y = mag; i->mBottomMost.y = -mag; i->mBounds.x1 = i->mBounds.y1 = mag; i->mBounds.x2 = i->mBounds.y2 = -mag; for (vector<cv::Point>::iterator pt = i->mContourPoints.begin(); pt != i->mContourPoints.end(); ++pt) { i->mCentroid.x += pt->x; i->mCentroid.y += pt->y; i->mBounds.include(Vec2f(pt->x, pt->y)); if (i->mLeftMost.x > pt->x) { i->mLeftMost.x = pt->x; i->mLeftMost.y = pt->y; } if (i->mRightMost.x < pt->x) { i->mRightMost.x = pt->x; i->mRightMost.y = pt->y; } if (i->mTopMost.y > pt->y) { i->mTopMost.x = pt->x; i->mTopMost.y = pt->y; } if (i->mBottomMost.y < pt->y) { i->mBottomMost.x = pt->x; i->mBottomMost.y = pt->y; } } float sz = i->mContourPoints.size(); if (sz > 0.0f) { i->mCentroid.x /= sz; i->mCentroid.y /= sz; } i->mZDist = *to8.getDataRed(i->mCentroid); // Loop through and do z calc float steps = 10.0f; Vec3f step = (i->mBottomMost - i->mTopMost) / steps; Vec3f sample = i->mTopMost; for (int x = 0; x < steps; x++) { if (thresh.at<uint8_t>(cv::Point(sample.x, sample.y)) > 0) { float val = *to8.getDataRed(Vec2f(sample.x, sample.y)); if (val < mDepthLowPass) i->mZDist = max(i->mZDist, val); } sample += step; } step = (i->mRightMost - i->mLeftMost) / steps; sample = i->mLeftMost; for (int x = 0; x < steps; x++) { if (thresh.at<uint8_t>(cv::Point(sample.x, sample.y)) > 0) { float val = *to8.getDataRed(Vec2f(sample.x, sample.y)); if (val < mDepthLowPass) i->mZDist = max(i->mZDist, val); } sample += step; } } sort(blobs.begin(), blobs.end(), SortDescendingZ()); { BlobLock b(mBlobMutex); mIndexFG = 1 - mIndexFG; } }