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;
}
}
void MotionTrackingTestApp::draw()
{
// gl::setViewport( getWindowBounds() );
// clear out the window with black
gl::clear( Color( 1, 1, 1 ) );
// if( mSurface ){
// if( mTexture ){
// mTexture->update( mSurface );
// } else {
// mTexture = gl::Texture::create( mSurface );
// }
// gl::draw( mTexture, mTexture->getBounds(), getWindowBounds() );
// }
if( mSurfaceDepth ){
if( mTextureDepth ){
mTextureDepth->update( Channel32f( mSurfaceDepth ) );
} else {
mTextureDepth = gl::Texture::create( Channel32f( mSurfaceDepth ) );
}
gl::color( Color::white() );
gl::draw( mTextureDepth, mTextureDepth->getBounds() );
}
gl::pushMatrices();
gl::translate( Vec2f( 320, 0 ) );
if( mSurfaceBlur ){
if( mTextureDepth ){
mTextureDepth->update( Channel32f( mSurfaceBlur ) );
} else {
mTextureDepth = gl::Texture::create( Channel32f( mSurfaceBlur ) );
}
gl::draw( mTextureDepth, mTextureDepth->getBounds() );
}
gl::translate( Vec2f( 0, 240 ) );
if( mSurfaceSubtract ){
if( mTextureDepth ){
mTextureDepth->update( Channel32f( mSurfaceSubtract ) );
} else {
mTextureDepth = gl::Texture::create( Channel32f( mSurfaceSubtract ) );
}
gl::draw( mTextureDepth, mTextureDepth->getBounds() );
}
gl::translate( Vec2f( -320, 0 ) );
for( ContourVector::iterator iter = mContours.begin(); iter != mContours.end(); ++iter ){
glBegin( GL_LINE_LOOP );
for( vector< cv::Point >::iterator pt = iter->begin(); pt != iter->end(); ++pt ){
gl::color( Color( 1.0f, 0.0f, 0.0f ) );
gl::vertex( fromOcv( *pt ) );
}
glEnd();
}
gl::translate( Vec2f( 0, 240 ) );
for( int i=0; i<mTrackedShapes.size(); i++){
glBegin( GL_POINTS );
for( int j=0; j<mTrackedShapes[i].hull.size(); j++ ){
gl::color( Color( 1.0f, 0.0f, 0.0f ) );
gl::vertex( fromOcv( mTrackedShapes[i].hull[j] ) );
}
glEnd();
}
gl::popMatrices();
mParams->draw();
}