Skip to content

Commit

Permalink
Works with two hands.
Browse files Browse the repository at this point in the history
  • Loading branch information
@bmwesting
bmwesting committed Apr 20, 2012
1 parent 9c89f8d commit b05ebbb
Show file tree
Hide file tree
Showing 2 changed files with 397 additions and 94 deletions.
218 changes: 124 additions & 94 deletions main.cpp
View file
Original file line number Original file line Diff line number Diff line change
Expand Up @@ -25,6 +25,16 @@ const unsigned int ROI_OFFSET = 70;
// median blur factor // median blur factor
const unsigned int MEDIAN_BLUR_K = 5; const unsigned int MEDIAN_BLUR_K = 5;


// grasping threshold
const double GRASPING_THRESH = 0.9;

// colors
const Scalar COLOR_BLUE = Scalar(240,40,0);
const Scalar COLOR_DARK_GREEN = Scalar(0, 128, 0);
const Scalar COLOR_LIGHT_GREEN = Scalar(0,255,0);
const Scalar COLOR_YELLOW = Scalar(0,128,200);
const Scalar COLOR_RED = Scalar(0,0,255);

// returns true if the hand is near the sensor area // returns true if the hand is near the sensor area
bool handApproachingDisplayPerimeter(float x, float y) bool handApproachingDisplayPerimeter(float x, float y)
{ {
Expand Down Expand Up @@ -106,132 +116,152 @@ int main(int argc, char** argv)


Mat depthRaw(YRES, XRES, CV_16UC1); Mat depthRaw(YRES, XRES, CV_16UC1);
Mat depthShow(YRES, XRES, CV_8UC1); Mat depthShow(YRES, XRES, CV_8UC1);
//Mat rightHand(ROI_OFFSET*2, ROI_OFFSET*2, CV_8UC1); // hand processing Mat handDebug;

// this vector holds the displayed images of the hands
vector<Mat> debugFrames;


// rectangle used to extract hand regions from depth map // rectangle used to extract hand regions from depth map
Rect roi; Rect roi;
roi.width = ROI_OFFSET*2; roi.width = ROI_OFFSET*2;
roi.height = ROI_OFFSET*2; roi.height = ROI_OFFSET*2;


namedWindow("depthFrame", CV_WINDOW_AUTOSIZE); namedWindow("depthFrame", CV_WINDOW_AUTOSIZE);
namedWindow("handFrame", CV_WINDOW_AUTOSIZE); namedWindow("leftHandFrame", CV_WINDOW_AUTOSIZE);
namedWindow("rightHandFrame", CV_WINDOW_AUTOSIZE);



int key = 0; int key = 0;
while(key != 27 && key != 'q') while(key != 27 && key != 'q')
{ {


sensor->waitForDeviceUpdateOnUser(); sensor->waitForDeviceUpdateOnUser();


int handDepth;
if(sensor->getNumTrackedUsers() > 0)
{
Skeleton skel = sensor->getSkeleton(sensor->getUID(0));
SkeletonPoint rightHand = skel.rightHand;
if(rightHand.confidence == 1.0)
{
handDepth = rightHand.z * (DEPTH_SCALE_FACTOR);

if(!handApproachingDisplayPerimeter(rightHand.x, rightHand.y))
{
roi.x = rightHand.x - ROI_OFFSET;
roi.y = rightHand.y - ROI_OFFSET;
//printf("Hand depth = %d\n", handDepth);
}
}
}
else
handDepth = -1;

// update 16 bit depth matrix // update 16 bit depth matrix
memcpy(depthRaw.data, sensor->getDepthData(), XRES*YRES*2); memcpy(depthRaw.data, sensor->getDepthData(), XRES*YRES*2);
depthRaw.convertTo(depthShow, CV_8U, DEPTH_SCALE_FACTOR); depthRaw.convertTo(depthShow, CV_8U, DEPTH_SCALE_FACTOR);


// extract hand from image for(int handI = 0; handI < 2; handI++)
Mat rightHandCpy(depthShow, roi); {
Mat rightHand = rightHandCpy.clone();

// binary threshold
if(handDepth != -1)
rightHand = (rightHand > (handDepth - BIN_THRESH_OFFSET)) & (rightHand < (handDepth + BIN_THRESH_OFFSET));

// last pre-filtering step, apply median blur
medianBlur(rightHand, rightHand, MEDIAN_BLUR_K);

// create debug image of thresholded hand and cvt to RGB so hints show
Mat rightHandDebug = rightHand.clone();
cvtColor(rightHandDebug, rightHandDebug, CV_GRAY2RGB);

std::vector< std::vector<Point> > contours;
findContours(rightHand, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

if (contours.size()) {
for (int i = 0; i < contours.size(); i++) {
vector<Point> contour = contours[i];
Mat contourMat = Mat(contour);
double cArea = contourArea(contourMat);


if(cArea > 2000) // likely the hand int handDepth;
if(sensor->getNumTrackedUsers() > 0)
{
Skeleton skel = sensor->getSkeleton(sensor->getUID(0));
SkeletonPoint hand;

if( handI == 0)
hand = skel.leftHand;
else
hand = skel.rightHand;
if(hand.confidence == 1.0)
{ {
Scalar center = mean(contourMat); handDepth = hand.z * (DEPTH_SCALE_FACTOR);
Point centerPoint = Point(center.val[0], center.val[1]);
if(!handApproachingDisplayPerimeter(hand.x, hand.y))
{
roi.x = hand.x - ROI_OFFSET;
roi.y = hand.y - ROI_OFFSET;
}
}
}
else
handDepth = -1;


// approximate the contour by a simple curve // extract hand from image
vector<Point> approxCurve; Mat handCpy(depthShow, roi);
approxPolyDP(contourMat, approxCurve, 10, true); Mat handMat = handCpy.clone();


vector< vector<Point> > debugContourV; // binary threshold
debugContourV.push_back(approxCurve); if(handDepth != -1)
drawContours(rightHandDebug, debugContourV, 0, Scalar(0, 128, 0), 3); handMat = (handMat > (handDepth - BIN_THRESH_OFFSET)) & (handMat < (handDepth + BIN_THRESH_OFFSET));


vector<int> hull; // last pre-filtering step, apply median blur
convexHull(Mat(approxCurve), hull, false, false); medianBlur(handMat, handMat, MEDIAN_BLUR_K);


// draw the hull points // create debug image of thresholded hand and cvt to RGB so hints show in color
for(int j = 0; j < hull.size(); j++) handDebug = handMat.clone();
{ debugFrames.push_back(handDebug);
int index = hull[j]; cvtColor(debugFrames[handI], debugFrames[handI], CV_GRAY2RGB);
circle(rightHandDebug, approxCurve[index], 3, Scalar(0,128,200), 2);
}


//Convexity Defects Processing - TODO Later std::vector< std::vector<Point> > contours;
vector<ConvexityDefect> convexDefects; findContours(handMat, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
findConvexityDefects(approxCurve, hull, convexDefects);
printf("Number of defects: %d.\n", (int) convexDefects.size());


for(int j = 0; j < convexDefects.size(); j++) if (contours.size()) {
{ for (int i = 0; i < contours.size(); i++) {
circle(rightHandDebug, convexDefects[j].depth_point, 3, Scalar(128,155,200), 2); vector<Point> contour = contours[i];
Mat contourMat = Mat(contour);
double cArea = contourArea(contourMat);


} if(cArea > 2000) // likely the hand

// assemble point set of convex hull
vector<Point> hullPoints;
for(int k = 0; k < hull.size(); k++)
{ {
int curveIndex = hull[k]; Scalar center = mean(contourMat);
Point p = approxCurve[curveIndex]; Point centerPoint = Point(center.val[0], center.val[1]);
hullPoints.push_back(p);
// approximate the contour by a simple curve
vector<Point> approxCurve;
approxPolyDP(contourMat, approxCurve, 10, true);

vector< vector<Point> > debugContourV;
debugContourV.push_back(approxCurve);
drawContours(debugFrames[handI], debugContourV, 0, COLOR_DARK_GREEN, 3);

vector<int> hull;
convexHull(Mat(approxCurve), hull, false, false);

// draw the hull points
for(int j = 0; j < hull.size(); j++)
{
int index = hull[j];
circle(debugFrames[handI], approxCurve[index], 3, COLOR_YELLOW, 2);
}

// find convexity defects
vector<ConvexityDefect> convexDefects;
findConvexityDefects(approxCurve, hull, convexDefects);
printf("Number of defects: %d.\n", (int) convexDefects.size());

for(int j = 0; j < convexDefects.size(); j++)
{
circle(debugFrames[handI], convexDefects[j].depth_point, 3, COLOR_BLUE, 2);

}

// assemble point set of convex hull
vector<Point> hullPoints;
for(int k = 0; k < hull.size(); k++)
{
int curveIndex = hull[k];
Point p = approxCurve[curveIndex];
hullPoints.push_back(p);
}

// area of hull and curve
double hullArea = contourArea(Mat(hullPoints));
double curveArea = contourArea(Mat(approxCurve));
double handRatio = curveArea/hullArea;

// hand is grasping
if(handRatio > GRASPING_THRESH)
circle(debugFrames[handI], centerPoint, 5, COLOR_LIGHT_GREEN, 5);
else
circle(debugFrames[handI], centerPoint, 5, COLOR_RED, 5);
} }

} // contour conditional
// area of hull and curve } // hands loop
double hullArea = contourArea(Mat(hullPoints));
double curveArea = contourArea(Mat(approxCurve));
double handRatio = curveArea/hullArea;
//printf("Area of approxContour: %f\n", curveArea);
//printf("Area of convexHull: %f\n", hullArea);

// hand is grasping
if(handRatio > 0.8)
circle(rightHandDebug, centerPoint, 5, Scalar(0,255,0), 5);
else
circle(rightHandDebug, centerPoint, 5, Scalar(0,0,255), 5);
}
}
} }


resize(rightHandDebug, rightHandDebug, Size(), 3, 3);
imshow("depthFrame", depthShow); imshow("depthFrame", depthShow);
imshow("handFrame", rightHandDebug);
if(debugFrames.size() >= 2 )
{
resize(debugFrames[0], debugFrames[0], Size(), 3, 3);
resize(debugFrames[1], debugFrames[1], Size(), 3, 3);
imshow("leftHandFrame", debugFrames[0]);
imshow("rightHandFrame", debugFrames[1]);
debugFrames.clear();
}



key = waitKey(10); key = waitKey(10);


Expand Down
Loading

0 comments on commit b05ebbb

Please sign in to comment.