// skeletonHand.cpp : Defines the entry point for the console application. // // STL Header #include "stdafx.h" #include <iostream> #include <vector> // OpenCV Header #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> #include <opencv2/imgproc/imgproc.hpp> // o1. OpenNI Header #include <OpenNI.h> // n1. NiTE Header #include <NiTE.h> // namespace using namespace std; using namespace openni; using namespace nite; using std::vector; const unsigned int ROI_OFFSET = 70; const unsigned int XRES = 640; const unsigned int YRES = 480; const unsigned int BIN_THRESH_OFFSET = 5; const unsigned int MEDIAN_BLUR_K = 5; const unsigned int GRASPING_THRESH = 0.9; //define color const cv::Scalar COLOR_DARK_GREEN = cv::Scalar (0 , 128, 0); const cv::Scalar COLOR_YELLOW = cv::Scalar (0 , 128, 200 ); const cv::Scalar COLOR_BLUE = cv::Scalar ( 240, 40, 0); const cv::Scalar COLOR_LIGHT_GREEN = cv::Scalar (0, 255 ,0); const cv::Scalar COLOR_RED = cv::Scalar ( 0, 0, 255); struct ConvexityDefect { cv::Point start; cv::Point end; cv::Point depth_point; float depth; }; bool handApproachingDisplayPerimeter(float x, float y) { return (x > (XRES - ROI_OFFSET)) || (x < (ROI_OFFSET)) || (y > (YRES - ROI_OFFSET)) || (y < (ROI_OFFSET)); } // //// Thanks to Jose Manuel Cabrera for part of this C++ wrapper function ////Convexity為凸的意思，Defect為缺陷的意思，hull為殼的意思 ////貌似這個函數在opencv中已經被實現了 // void findConvexityDefects(vector<cv::Point>& contour, vector<int>& hull, vector<ConvexityDefect>& convexDefects) // { // if(hull.size() > 0 && contour.size() > 0) // { // CvSeq* contourPoints; // CvSeq* defects; // CvMemStorage* storage; // CvMemStorage* strDefects; // CvMemStorage* contourStr; // CvConvexityDefect *defectArray = 0; // // strDefects = cvCreateMemStorage(); // defects = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq),sizeof(CvPoint), strDefects ); // // //We transform our vector<Point> into a CvSeq* object of CvPoint. // contourStr = cvCreateMemStorage(); // contourPoints = cvCreateSeq(CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), contourStr); // for(int i = 0; i < (int)contour.size(); i++) { // CvPoint cp = {contour[i].x, contour[i].y}; // cvSeqPush(contourPoints, &cp); // } // // //Now, we do the same thing with the hull index // int count = (int) hull.size(); // //int hullK[count]; // int* hullK = (int*) malloc(count*sizeof(int)); // for(int i = 0; i < count; i++) { hullK[i] = hull.at(i); } // CvMat hullMat = cvMat(1, count, CV_32SC1, hullK); // // // calculate convexity defects // storage = cvCreateMemStorage(0); // defects = cv::convexityDefects(*contourPoints, &hullMat, *storage); // defectArray = (CvConvexityDefect*)malloc(sizeof(CvConvexityDefect)*defects->total); // cvCvtSeqToArray(defects, defectArray, CV_WHOLE_SEQ); // //printf("DefectArray %i %i\n",defectArray->end->x, defectArray->end->y); // // //We store defects points in the convexDefects parameter. // for(int i = 0; i<defects->total; i++){ // ConvexityDefect def; // def.start = cv::Point(defectArray[i].start->x, defectArray[i].start->y); // def.end = cv::Point(defectArray[i].end->x, defectArray[i].end->y); // def.depth_point = cv::Point(defectArray[i].depth_point->x, defectArray[i].depth_point->y); // def.depth = defectArray[i].depth; // convexDefects.push_back(def); // } // // // release memory // cvReleaseMemStorage(&contourStr); // cvReleaseMemStorage(&strDefects); // cvReleaseMemStorage(&storage); // // } //} // // // int main( int argc, char **argv ) { // o2. Initial OpenNI OpenNI::initialize(); // o3. Open Device Device mDevice; mDevice.open( ANY_DEVICE ); // o4. create depth stream VideoStream mDepthStream; mDepthStream.create( mDevice, SENSOR_DEPTH ); // o4a. set video mode VideoMode mDMode; mDMode.setResolution( 640, 480 ); mDMode.setFps( 30 ); mDMode.setPixelFormat( PIXEL_FORMAT_DEPTH_1_MM ); mDepthStream.setVideoMode( mDMode); // o5. Create color stream VideoStream mColorStream; mColorStream.create( mDevice, SENSOR_COLOR ); // o5a. set video mode VideoMode mCMode; mCMode.setResolution( 640, 480 ); mCMode.setFps( 30 ); mCMode.setPixelFormat( PIXEL_FORMAT_RGB888 ); mColorStream.setVideoMode( mCMode); // o6. image registration mDevice.setImageRegistrationMode( IMAGE_REGISTRATION_DEPTH_TO_COLOR ); // n2. Initial NiTE NiTE::initialize(); // n3. create user tracker UserTracker mUserTracker; mUserTracker.create( &mDevice ); mUserTracker.setSkeletonSmoothingFactor( 0.1f ); //set ROI cv::Rect ROI; ROI.width = ROI_OFFSET * 2; ROI.height = ROI_OFFSET * 2; // create OpenCV Window cv::namedWindow( "User Image", CV_WINDOW_AUTOSIZE ); cv::namedWindow( "Depth Image", CV_WINDOW_AUTOSIZE ); cv::namedWindow( "leftHandFrame Image", CV_WINDOW_AUTOSIZE ); cv::namedWindow( "rightHandFrame Image", CV_WINDOW_AUTOSIZE ); // p1. start mColorStream.start(); mDepthStream.start(); int iMaxDepth = mDepthStream.getMaxPixelValue(); while( true ) { // main loop cv::Mat cImageBGR; cv::Mat cImageDepth; cv::Mat handDebug; vector< cv::Mat > debugFrames; // p2a. get color frame VideoFrameRef mColorFrame; VideoFrameRef mDepthFrame; mColorStream.readFrame( &mColorFrame ); mDepthStream.readFrame( &mDepthFrame ); // p2b. convert data to OpenCV format const cv::Mat mImageRGB( mColorFrame.getHeight(), mColorFrame.getWidth(), CV_8UC3, (void*)mColorFrame.getData() ); // p2c. convert form RGB to BGR cv::cvtColor( mImageRGB, cImageBGR, CV_RGB2BGR ); // now we do same thing to the depth const cv::Mat mImageDepth( mDepthFrame.getHeight(), mDepthFrame.getWidth(), CV_16UC1, (void*)mDepthFrame.getData() ); // 8c. re-map depth data [0,Max] to [0,255] mImageDepth.convertTo( cImageDepth, CV_8U, 255.0 / iMaxDepth ); // p3. get user frame UserTrackerFrameRef mUserFrame; mUserTracker.readFrame( &mUserFrame ); // p4. get users data const nite::Array<UserData>& aUsers = mUserFrame.getUsers(); for( int i = 0; i < aUsers.getSize(); ++ i ) { const UserData& rUser = aUsers[i]; // p4a. check user status if( rUser.isNew() ) { // start tracking for new user mUserTracker.startSkeletonTracking( rUser.getId() ); } if( rUser.isVisible() ) { // p4b. get user skeleton const Skeleton& rSkeleton = rUser.getSkeleton(); int handDepth = -1; if ( rSkeleton.getState() == SKELETON_TRACKED ) { for ( int handI = 0; handI < 2; handI++) { //get the position of the hand //struct SkeletonJoint //{ // float x, y, z, confidence; //}; SkeletonJoint hand; if ( handI == 0) { hand = rSkeleton.getJoint( JOINT_RIGHT_HAND ); } else { hand = rSkeleton.getJoint( JOINT_LEFT_HAND ); } if ( hand.getPositionConfidence() >= 0.5 ) { const Point3f& handPosition = hand.getPosition(); //change to 8 bites image handDepth = handPosition.z * 255.0 / iMaxDepth; //get the roi if(!handApproachingDisplayPerimeter(handPosition.x, handPosition.y)) { ROI.x = handPosition.x - ROI_OFFSET; ROI.y = handPosition.y - ROI_OFFSET; } } // set the ROI in depth image cv::Mat handCpy( cImageDepth, ROI ); cv::Mat handMat = handCpy.clone (); //binary threshold if ( handDepth != -1) handMat = (handMat > (handDepth - BIN_THRESH_OFFSET)) & (handMat < (handDepth + BIN_THRESH_OFFSET)); //median blur medianBlur(handMat, handMat, MEDIAN_BLUR_K); handDebug = handMat.clone(); debugFrames.push_back(handDebug); cvtColor(debugFrames[handI], debugFrames[handI], CV_GRAY2RGB); std::vector< std::vector < cv::Point >> contours; std::vector<cv::Vec4i> hierarchy; findContours(handMat, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE); if (contours.size()) { for (int i = 0; i < contours.size(); i++) { vector<cv::Point> contour = contours[i]; //将vector转换成Mat型，此时的Mat还是列向量，只不过是2个通道的列向量而已 cv::Mat contourMat = cv::Mat(contour); //返回轮廓的面积 double cArea = contourArea(contourMat); if(cArea > 2000) { cv::Scalar center = mean(contourMat); cv::Point centerPoint = cv::Point(center.val[0], center.val[1]); // approximate the contour by a simple curve vector<cv::Point> approxCurve; //求出轮廓的封闭的曲线，保存在approxCurve，轮廓和封闭曲线直接的最大距离为10 approxPolyDP(contourMat, approxCurve, 10, true); vector< vector<cv::Point> > debugContourV; debugContourV.push_back(approxCurve); //在参数1中画出轮廓参数2，参数2必须是轮廓的集合，所以参数2是 //vector< vector<Point> >类型 //深绿色代表近似多边形 drawContours(debugFrames[handI], debugContourV, 0, COLOR_DARK_GREEN , 3); vector<int> hull; //找出近似曲线的凸包集合，集合hull中存储的是轮廓中凸包点的下标 convexHull(cv::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<cv::Vec4i> convexDefects; cv::convexityDefects(cv::Mat(approxCurve), hull, convexDefects); printf("Number of defects: %d.\n", (int) convexDefects.size()); for(int j = 0; j < convexDefects.size(); j++) { //缺陷点用蓝色表示 circle(debugFrames[handI], approxCurve[convexDefects[j][2]], 3, COLOR_BLUE, 2); } // assemble point set of convex hull //将凸包集以点的坐标形式保存下来 vector<cv::Point> hullPoints; for(int k = 0; k < hull.size(); k++) { int curveIndex = hull[k]; cv::Point p = approxCurve[curveIndex]; hullPoints.push_back(p); } // area of hull and curve double hullArea = contourArea(cv::Mat(hullPoints)); double curveArea = contourArea(cv::Mat(approxCurve)); double handRatio = curveArea/hullArea; // hand is grasping //GRASPING_THRESH == 0.9 if(handRatio > GRASPING_THRESH) //握拳表示绿色 circle(debugFrames[handI], centerPoint, 5, COLOR_LIGHT_GREEN, 5); else //一般情况下手张开其中心点是显示红色 circle(debugFrames[handI], centerPoint, 5, COLOR_RED, 5); } } } } } } } // p5. show image cv::imshow( "User Image", cImageBGR ); cv::imshow( "Depth Image", cImageDepth ); //debugFrames只保存2帧图像 if(debugFrames.size() >= 2 ) { //长和宽的尺寸都扩大3倍 resize(debugFrames[0], debugFrames[0], cv::Size(), 3, 3); resize(debugFrames[1], debugFrames[1], cv::Size(), 3, 3); imshow("leftHandFrame Image", debugFrames[0]); imshow("rightHandFrame Image", debugFrames[1]); debugFrames.clear(); } cv::waitKey (20); // p6. check keyboard if( cv::waitKey( 1 ) == 'q' ) break; } // p7. stop mUserTracker.destroy(); mColorStream.destroy(); mDepthStream.destroy(); mDevice.close(); NiTE::shutdown(); OpenNI::shutdown(); return 0; }