void cvUpdateTracks(CvBlobs const &blobs, CvTracks &tracks, const double thDistance, const unsigned int thInactive, const unsigned int thActive) { CV_FUNCNAME("cvUpdateTracks"); __CV_BEGIN__; unsigned int nBlobs = blobs.size(); unsigned int nTracks = tracks.size(); // Proximity matrix: // Last row/column is for ID/label. // Last-1 "/" is for accumulation. CvID *close = new unsigned int[(nBlobs+2)*(nTracks+2)]; // XXX Must be same type than CvLabel. try { // Inicialization: unsigned int i=0; for (CvBlobs::const_iterator it = blobs.begin(); it!=blobs.end(); ++it, i++) { AB(i) = 0; IB(i) = it->second->label; } CvID maxTrackID = 0; unsigned int j=0; for (CvTracks::const_iterator jt = tracks.begin(); jt!=tracks.end(); ++jt, j++) { AT(j) = 0; IT(j) = jt->second->id; if (jt->second->id > maxTrackID) maxTrackID = jt->second->id; } // Proximity matrix calculation and "used blob" list inicialization: for (i=0; i<nBlobs; i++) for (j=0; j<nTracks; j++) if (C(i, j) = (distantBlobTrack(B(i), T(j)) < thDistance)) { AB(i)++; AT(j)++; } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Detect inactive tracks for (j=0; j<nTracks; j++) { unsigned int c = AT(j); if (c==0) { //cout << "Inactive track: " << j << endl; // Inactive track. CvTrack *track = T(j); track->inactive++; track->label = 0; } } // Detect new tracks for (i=0; i<nBlobs; i++) { unsigned int c = AB(i); if (c==0) { //cout << "Blob (new track): " << maxTrackID+1 << endl; //cout << *B(i) << endl; // New track. maxTrackID++; CvBlob *blob = B(i); CvTrack *track = new CvTrack; track->id = maxTrackID; track->label = blob->label; track->minx = blob->minx; track->miny = blob->miny; track->maxx = blob->maxx; track->maxy = blob->maxy; track->centroid = blob->centroid; track->lifetime = 0; track->active = 0; track->inactive = 0; tracks.insert(CvIDTrack(maxTrackID, track)); } } // Clustering for (j=0; j<nTracks; j++) { unsigned int c = AT(j); if (c) { list<CvTrack*> tt; tt.push_back(T(j)); list<CvBlob*> bb; getClusterForTrack(j, close, nBlobs, nTracks, blobs, tracks, bb, tt); // Select track CvTrack *track; unsigned int area = 0; for (list<CvTrack*>::const_iterator it=tt.begin(); it!=tt.end(); ++it) { CvTrack *t = *it; unsigned int a = (t->maxx-t->minx)*(t->maxy-t->miny); if (a>area) { area = a; track = t; } } // Select blob CvBlob *blob; area = 0; //cout << "Matching blobs: "; for (list<CvBlob*>::const_iterator it=bb.begin(); it!=bb.end(); ++it) { CvBlob *b = *it; //cout << b->label << " "; if (b->area>area) { area = b->area; blob = b; } } //cout << endl; // Update track //cout << "Matching: track=" << track->id << ", blob=" << blob->label << endl; track->label = blob->label; track->centroid = blob->centroid; track->minx = blob->minx; track->miny = blob->miny; track->maxx = blob->maxx; track->maxy = blob->maxy; if (track->inactive) track->active = 0; track->inactive = 0; // Others to inactive for (list<CvTrack*>::const_iterator it=tt.begin(); it!=tt.end(); ++it) { CvTrack *t = *it; if (t!=track) { //cout << "Inactive: track=" << t->id << endl; t->inactive++; t->label = 0; } } } } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// for (CvTracks::iterator jt=tracks.begin(); jt!=tracks.end();) if ((jt->second->inactive>=thInactive)||((jt->second->inactive)&&(thActive)&&(jt->second->active<thActive))) { delete jt->second; tracks.erase(jt++); } else { jt->second->lifetime++; if (!jt->second->inactive) jt->second->active++; ++jt; } } catch (...) { delete[] close; throw; // TODO: OpenCV style. } delete[] close; __CV_END__; }
unsigned int cvLabel (IplImage const *img, IplImage *imgOut, CvBlobs &blobs) { CV_FUNCNAME("cvLabel"); __CV_BEGIN__; { CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==1)); CV_ASSERT(imgOut&&(imgOut->depth==IPL_DEPTH_LABEL)&&(imgOut->nChannels==1)); unsigned int numPixels=0; cvSetZero(imgOut); CvLabel label=0; cvReleaseBlobs(blobs); unsigned int stepIn = img->widthStep / (img->depth / 8); unsigned int stepOut = imgOut->widthStep / (imgOut->depth / 8); unsigned int imgIn_width = img->width; unsigned int imgIn_height = img->height; unsigned int imgIn_offset = 0; // unsigned int imgOut_width = imgOut->width; // Unused but set // unsigned int imgOut_height = imgOut->height; // Unused but set unsigned int imgOut_offset = 0; if(img->roi) { imgIn_width = img->roi->width; imgIn_height = img->roi->height; imgIn_offset = img->roi->xOffset + (img->roi->yOffset * stepIn); } if(imgOut->roi) { // imgOut_width = imgOut->roi->width; // Unused but set // imgOut_height = imgOut->roi->height; // Unused but set imgOut_offset = imgOut->roi->xOffset + (imgOut->roi->yOffset * stepOut); } unsigned char *imgDataIn = (unsigned char *)img->imageData + imgIn_offset; CvLabel *imgDataOut = (CvLabel *)imgOut->imageData + imgOut_offset; #define imageIn(X, Y) imgDataIn[(X) + (Y)*stepIn] #define imageOut(X, Y) imgDataOut[(X) + (Y)*stepOut] CvLabel lastLabel = 0; CvBlob *lastBlob = NULL; for (unsigned int y=0; y<imgIn_height; y++) { for (unsigned int x=0; x<imgIn_width; x++) { if (imageIn(x, y)) { bool labeled = imageOut(x, y); if ((!imageOut(x, y))&&((y==0)||(!imageIn(x, y-1)))) { labeled = true; // Label contour. label++; CV_ASSERT(label!=CV_BLOB_MAX_LABEL); imageOut(x, y) = label; numPixels++; // XXX This is not necessary at all. I only do this for consistency. if (y>0) imageOut(x, y-1) = CV_BLOB_MAX_LABEL; CvBlob *blob = new CvBlob; blob->label = label; blob->area = 1; blob->minx = x; blob->maxx = x; blob->miny = y; blob->maxy = y; blob->m10=x; blob->m01=y; blob->m11=x*y; blob->m20=x*x; blob->m02=y*y; blob->internalContours.clear(); blobs.insert(CvLabelBlob(label,blob)); lastLabel = label; lastBlob = blob; blob->contour.startingPoint = cvPoint(x, y); unsigned char direction=1; unsigned int xx = x; unsigned int yy = y; bool contourEnd = false; do { for (unsigned int numAttempts=0; numAttempts<3; numAttempts++) { bool found = false; for (unsigned char i=0; i<3; i++) { unsigned int nx = xx + movesE[direction][i][0]; unsigned int ny = yy + movesE[direction][i][1]; if ((nx < imgIn_width) && (nx >= 0) && (ny < imgIn_height) && (ny >= 0)) { if (imageIn(nx, ny)) { found = true; blob->contour.chainCode.push_back(movesE[direction][i][3]); xx = nx; yy = ny; direction = movesE[direction][i][2]; break; } else { imageOut(nx, ny) = CV_BLOB_MAX_LABEL; } } } if (!found) direction = (direction + 1) % 4; else { if (imageOut(xx, yy) != label) { imageOut(xx, yy) = label; numPixels++; if (xx < blob->minx) blob->minx = xx; else if (xx > blob->maxx) blob->maxx = xx; if (yy < blob->miny) blob->miny = yy; else if (yy > blob->maxy) blob->maxy = yy; blob->area++; blob->m10 += xx; blob->m01 += yy; blob->m11 += xx*yy; blob->m20 += xx*xx; blob->m02 += yy * yy; } break; } if ( (contourEnd = ((xx == x) && (yy == y) && (direction == 1))) ) break; } } while (!contourEnd); } if ((y + 1 < imgIn_height) && (!imageIn(x, y + 1)) && (!imageOut(x, y + 1))) { labeled = true; // Label internal contour CvLabel l; CvBlob *blob = NULL; if (!imageOut(x, y)) { /*if (!imageOut(x-1, y)) { cerr << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl; continue; }*/ l = imageOut(x - 1, y); imageOut(x, y) = l; numPixels++; if (l==lastLabel) blob = lastBlob; else { blob = blobs.find(l)->second; lastLabel = l; lastBlob = blob; } blob->area++; blob->m10 += x; blob->m01 += y; blob->m11 += x*y; blob->m20 += x*x; blob->m02 += y*y; } else { l = imageOut(x, y); if (l==lastLabel) blob = lastBlob; else { blob = blobs.find(l)->second; lastLabel = l; lastBlob = blob; } } // XXX This is not necessary (I believe). I only do this for consistency. imageOut(x, y + 1) = CV_BLOB_MAX_LABEL; CvContourChainCode *contour = new CvContourChainCode; contour->startingPoint = cvPoint(x, y); unsigned char direction = 3; unsigned int xx = x; unsigned int yy = y; do { for (unsigned int numAttempts=0; numAttempts<3; numAttempts++) { bool found = false; for (unsigned char i=0; i<3; i++) { unsigned int nx = xx + movesI[direction][i][0]; unsigned int ny = yy + movesI[direction][i][1]; if (imageIn(nx, ny)) { found = true; contour->chainCode.push_back(movesI[direction][i][3]); xx=nx; yy=ny; direction=movesI[direction][i][2]; break; } else { imageOut(nx, ny) = CV_BLOB_MAX_LABEL; } } if (!found) direction=(direction+1)%4; else { if (!imageOut(xx, yy)) { imageOut(xx, yy) = l; numPixels++; blob->area++; blob->m10 += xx; blob->m01 += yy; blob->m11 += xx*yy; blob->m20 += xx*xx; blob->m02 += yy*yy; } break; } } } while (!(xx==x && yy==y)); blob->internalContours.push_back(contour); } //else if (!imageOut(x, y)) if (!labeled) { // Internal pixel CvLabel l = imageOut(x-1, y); imageOut(x, y) = l; numPixels++; CvBlob *blob = NULL; if (l==lastLabel) blob = lastBlob; else { blob = blobs.find(l)->second; lastLabel = l; lastBlob = blob; } blob->area++; blob->m10 += x; blob->m01 += y; blob->m11 += x*y; blob->m20 += x*x; blob->m02 += y*y; } } } } for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it) { cvCentroid((*it).second); (*it).second->u11 = (*it).second->m11 - ((*it).second->m10*(*it).second->m01)/(*it).second->m00; (*it).second->u20 = (*it).second->m20 - ((*it).second->m10*(*it).second->m10)/(*it).second->m00; (*it).second->u02 = (*it).second->m02 - ((*it).second->m01*(*it).second->m01)/(*it).second->m00; double m00_2 = (*it).second->m00 * (*it).second->m00; (*it).second->n11 = (*it).second->u11 / m00_2; (*it).second->n20 = (*it).second->u20 / m00_2; (*it).second->n02 = (*it).second->u02 / m00_2; (*it).second->p1 = (*it).second->n20 + (*it).second->n02; double nn = (*it).second->n20 - (*it).second->n02; (*it).second->p2 = nn*nn + 4.*((*it).second->n11*(*it).second->n11); } return numPixels; } __CV_END__; return 0; // Remove return warning }
void processVideo(char* videoFilename) { //create the capture object IplImage *labelImg;//foreground CTracker openTracker((float)0.033, (float)0.6, (double)20.0, 10, 3000); CvTracks tracks; VideoCapture capture(videoFilename); if(!capture.isOpened()){ //error in opening the video input cerr << "Unable to open video file: " << videoFilename << endl; exit(EXIT_FAILURE); } bool bInitialized = false; //read input data. ESC or 'q' for quitting while( (char)keyboard != 'q' && (char)keyboard != 27 ){ //read the current frame if(!capture.read(frame)) { cerr << "Unable to read next frame." << endl; cerr << "Exiting..." << endl; exit(EXIT_FAILURE); } if(bInitialized==false) { cv::Size frameSize(static_cast<int>(frame.cols), static_cast<int>(frame.rows)); labelImg = cvCreateImage(frameSize, IPL_DEPTH_LABEL, 1); bInitialized = true; } //update the background model pMOG2.operator ()(frame,fgMaskMOG2); //open operator. cv::erode(fgMaskMOG2,fgMaskMOG2,cv::Mat(),cv::Point(-1,-1),1); cv::dilate(fgMaskMOG2,fgMaskMOG2,cv::Mat(),cv::Point(-1,-1),4); // step 2::blob analysis CvBlobs blobs; unsigned int result = cvLabel(&(IplImage)fgMaskMOG2, labelImg, blobs); cvFilterByArea(blobs, 125, 10000); cvRenderBlobs(labelImg, blobs, &(IplImage)frame, &(IplImage)frame, CV_BLOB_RENDER_BOUNDING_BOX); //cvUpdateTracks(blobs, tracks, 200, 5); //cvRenderTracks(tracks, &(IplImage)frame, &(IplImage)frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX|CV_TRACK_RENDER_TO_LOG); //convert the blobs into detection structure; vector<Detection*> detections; for (CvBlobs::const_iterator it=blobs.begin();it!=blobs.end();++it) { CvBlob *blob=(*it).second; Detection *_detection = new Detection; _detection->centroid.x= blob->centroid.x; _detection->centroid.y= blob->centroid.y; _detection->brect.x = blob->minx; _detection->brect.y = blob->miny; _detection->brect.height = blob->maxy - blob->miny; _detection->brect.width = blob->maxx - blob->minx; detections.push_back(_detection); } //Step 3 : give the list of all centroids of all detected contours to tracker. Track return the trace of the track, whose values are Kalman-filtered if(blobs.size() > 0) { openTracker.Update(detections); int i, j; for(i=0; i < openTracker.tracks.size(); i++) { //add a threshold to de-noise, if the contour just appeared, maybe noise. set a threshold if(openTracker.tracks[i]->trace.size() > 10) { for(j = 0; j < (openTracker.tracks[i]->trace.size() - 2); j++) { cv::rectangle(frame, openTracker.tracks[i]->brect, Scalar(255,0,0)); //line(fore, openTracker.tracks[i]->trace[j], openTracker.tracks[i]->trace[j+1], Colors[openTracker.tracks[i]->track_id % 9], 1, CV_AA); line(frame, openTracker.tracks[i]->trace[j], openTracker.tracks[i]->trace[j+1], Scalar(255,0,0), 1, CV_AA); } stringstream ss; ss << openTracker.tracks[i]->track_id; string str = ss.str(); putText(frame, str, openTracker.tracks[i]->trace[j], FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255,0,0), 1); } } } //get the frame number and write it on the current frame stringstream ss; rectangle(frame, cv::Point(10, 2), cv::Point(100,20),cv::Scalar(255,255,255), -1); //show the current frame and the fg masks imshow("Frame", frame); imshow("FG Mask MOG 2", fgMaskMOG2); //get the input from the keyboard keyboard = waitKey( 30 ); } //delete capture object capture.release(); cvReleaseImage(&labelImg); }
void MultiCursorAppCpp::setCursor(CvBlobs blobs) { #ifdef USE_KINECT_V1 INT blobID = 0; for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) { if (userData[blobID].handInfo.isTracked) { Mat handPoint = (cv::Mat_<float>(4, 1) << userData[blobID].handInfo.cameraPoint.x, userData[blobID].handInfo.cameraPoint.y, userData[blobID].handInfo.cameraPoint.z, 1); Mat headPoint = (cv::Mat_<float>(4, 1) << userData[blobID].headInfo.cameraPoint.x, userData[blobID].headInfo.cameraPoint.y, userData[blobID].headInfo.cameraPoint.z, 1); Mat handPointScreen = T_WorldToScreen * T_KinectCameraToWorld * handPoint; Mat headPointScreen = T_WorldToScreen * T_KinectCameraToWorld * headPoint; // Caliculate the intersection point of vector and screen float xvec = *handPointScreen.ptr<float>(0, 0) - *headPointScreen.ptr<float>(0, 0); float yvec = *handPointScreen.ptr<float>(1, 0) - *headPointScreen.ptr<float>(1, 0); float zvec = *handPointScreen.ptr<float>(2, 0) - *headPointScreen.ptr<float>(2, 0); float val = -*handPointScreen.ptr<float>(2, 0) / zvec; // Calculate cursor position in real scall Point3f cursorScreen3d; cursorScreen3d.x = val * xvec + *headPointScreen.ptr<float>(0, 0); cursorScreen3d.y = val * yvec + *headPointScreen.ptr<float>(1, 0); cursorScreen3d.z = 0.0f; // Calculate cursor position in pixel coordinate float screen3dTo2d = 246 / 0.432f; userData[blobID].cursorInfo.position.x = cursorScreen3d.x * screen3dTo2d; userData[blobID].cursorInfo.position.y = cursorScreen3d.y * screen3dTo2d; userData[blobID].cursorInfo.isShownCursor; } ++blobID; } #else INT blobID = 0; for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) { userData[blobID].cursorInfo.isShownCursor = false; if (userData[blobID].handInfo.isTracked) { Mat handPoint = (cv::Mat_<float>(4, 1) << userData[blobID].handInfo.cameraPoint.x * 1000, userData[blobID].handInfo.cameraPoint.y * 1000, userData[blobID].handInfo.cameraPoint.z * 1000, 1); Mat headPoint = (cv::Mat_<float>(4, 1) << userData[blobID].headInfo.cameraPoint.x * 1000, userData[blobID].headInfo.cameraPoint.y * 1000, userData[blobID].headInfo.cameraPoint.z * 1000, 1); for (size_t i = 0; i < TKinect2Display.size(); ++i) { //Mat handPointScreen = TMarker2Display * TKinect2Marker * TKinectDepth2Color * handPoint; //Mat headPointScreen = TMarker2Display * TKinect2Marker * TKinectDepth2Color * headPoint; Mat handPointScreen = TKinect2Display[i] * handPoint; Mat headPointScreen = TKinect2Display[i] * headPoint; // Caliculate the intersection point of vector and screen float xvec = *handPointScreen.ptr<float>(0, 0) - *headPointScreen.ptr<float>(0, 0); float yvec = *handPointScreen.ptr<float>(1, 0) - *headPointScreen.ptr<float>(1, 0); float zvec = *handPointScreen.ptr<float>(2, 0) - *headPointScreen.ptr<float>(2, 0); float val = -*handPointScreen.ptr<float>(2, 0) / zvec; // Calculate cursor position in real scall Point3f cursorScreen3d; cursorScreen3d.x = val * xvec + *headPointScreen.ptr<float>(0, 0); cursorScreen3d.y = val * yvec + *headPointScreen.ptr<float>(1, 0); cursorScreen3d.z = 0.0f; // Calculate cursor position in pixel coordinate Mat cursor3d = (Mat_<float>(3, 1) << cursorScreen3d.x, cursorScreen3d.y, 1); Mat cursor2d = TDisplay2Pixel[i] * cursor3d; if (i == 1) { cursor2d.at<float>(1, 0) = -cursor2d.at<float>(1, 0) + 40000; } //cursor2d /= *cursor2d.ptr<float>(2, 1); if (0 < *cursor2d.ptr<float>(0, 0) && *cursor2d.ptr<float>(0, 0) < VEC_WIN_WIDTH[0] && 0 < *cursor2d.ptr<float>(1, 0) && *cursor2d.ptr<float>(1, 0) < VEC_WIN_HEIGHT[0]) { userData[blobID].cursorInfo.isShownCursor = true; userData[blobID].cursorInfo.displayNum = i; userData[blobID].cursorInfo.position.x = *cursor2d.ptr<float>(0, 0); userData[blobID].cursorInfo.position.y = *cursor2d.ptr<float>(1, 0); } if (i == 1) cout << cursor3d << endl; } } ++blobID; } #endif }
void MultiCursorAppCpp::detectHandPosition(CvBlobs blobs) { FLOAT offset = 0.03f; INT blobID = 0; for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) { int numIntersectionPoints = 0; Vector4 handPosition; handPosition.w = 1; handPosition.x = 0.0f; handPosition.y = 0.0f; handPosition.z = 0.0f; Point3f center3f = Point3_<FLOAT>(userData[blobID].headInfo.cameraPoint.x, userData[blobID].headInfo.cameraPoint.y, userData[blobID].headInfo.cameraPoint.z); // ユーザの領域内を探索 for (int y = it->second->miny; y <= it->second->maxy; y++) { for (int x = it->second->minx; x <= it->second->maxx; x++) { float length = sqrt( pow(center3f.x - point3fMatrix.ptr<float>(y, x)[0], 2) + pow(center3f.y - point3fMatrix.ptr<float>(y, x)[1], 2) //+ pow(center3f.z - point3fMatrix.ptr<float>(y, x)[2], 2) ); // Define the intersection point of the sphere which its center is head and the hand as the hand position if (*heightMatrix.ptr<USHORT>(y, x) > userData[blobID].headInfo.height - HEAD_LENGTH - SHOULDER_LENGTH // 肩より高い点か && 0 < point3fMatrix.ptr<float>(y, x)[2]*1000 && point3fMatrix.ptr<float>(y, x)[2] * 1000 < KINECT_HEIGHT // Kinectの検出できる範囲の値か && it->first == labelMat.at<unsigned long>(y, x) // 同じblob内のみ探索 ) // Don't include desk { if (SENCIG_CIRCLE_RADIUS - offset < length ) { // 注目点が球と交差しているかどうか handPosition.x += point3fMatrix.ptr<float>(y, x)[0]; handPosition.y += point3fMatrix.ptr<float>(y, x)[1]; handPosition.z += point3fMatrix.ptr<float>(y, x)[2]; circle(userAreaMat, Point(x, y), 2, Scalar(255, 255, 0), -1); numIntersectionPoints++; } else if (length > SENCIG_CIRCLE_RADIUS) { #ifdef TRACK_GESTURE_BY_AREA ++userData[blobID].handInfo.area; // 手の面積を点の数で表す #ifdef USE_KINECT_V1 LONG handPositionX2d; LONG handPositionY2d; USHORT dis; NuiTransformSkeletonToDepthImage(handPosition, &handPositionX2d, &handPositionY2d, &dis, KINECT_RESOLUTION); circle(userAreaMat, Point(handPositionX2d, handPositionY2d), 7, Scalar(0, 200, 0), 3); #else DepthSpacePoint depthPoint; CameraSpacePoint cameraPoint; cameraPoint.X = point3fMatrix.ptr<float>(y, x)[0]; cameraPoint.Y = point3fMatrix.ptr<float>(y, x)[1]; cameraPoint.Z = point3fMatrix.ptr<float>(y, x)[2]; kinectBasics.GetMapper()->MapCameraPointToDepthSpace(cameraPoint, &depthPoint); int index = ((y * kinectBasics.widthDepth) + x) * 3; UCHAR* dataDepth = &userAreaMat.data[index]; dataDepth[0] = 0; dataDepth[1] = 200; dataDepth[2] = 255; //circle(userAreaMat, Point(depthPoint.X, depthPoint.Y), 1, Scalar(0, 255, 255), 3); #endif #endif } } } } if (numIntersectionPoints > 0) { userData[blobID].handInfo.isTracked = true; handPosition.x /= numIntersectionPoints; handPosition.y /= numIntersectionPoints; handPosition.z /= numIntersectionPoints; userData[blobID].handInfo.cameraPoint.x = handPosition.x; userData[blobID].handInfo.cameraPoint.y = handPosition.y; userData[blobID].handInfo.cameraPoint.z = handPosition.z; // Show the hand positions on the depth image #ifdef USE_KINECT_V1 LONG handPositionX2d; LONG handPositionY2d; USHORT dis; NuiTransformSkeletonToDepthImage(handPosition, &handPositionX2d, &handPositionY2d, &dis, KINECT_RESOLUTION); circle(userAreaMat, Point(handPositionX2d, handPositionY2d), 7, Scalar(0, 200, 0), 3); #else DepthSpacePoint depthPoint; CameraSpacePoint cameraPoint; cameraPoint.X = handPosition.x; cameraPoint.Y = handPosition.y; cameraPoint.Z = handPosition.z; kinectBasics.GetMapper()->MapCameraPointToDepthSpace(cameraPoint, &depthPoint); circle(userAreaMat, Point(depthPoint.X, depthPoint.Y), 7, Scalar(0, 200, 0), 3); #endif } else { userData[blobID].handInfo.isTracked = false; userData[blobID].handInfo.cameraPoint.x = 0.0f; userData[blobID].handInfo.cameraPoint.y = 0.0f; userData[blobID].handInfo.cameraPoint.z = 0.0f; } #ifdef TRACK_GESTURE_BY_AREA // 面積で手の開閉をチェック if (userData[blobID].handInfo.area != 0) { float distance = sqrt(pow(handPosition.x, 2) + pow(handPosition.y, 2) + pow(handPosition.z, 2)); userData[blobID].handInfo.area /= distance; } // cout << "area: " << userData[blobID].handInfo.area << endl; #endif blobID++; } // Replace preUserData by current userData / 前フレームのデータを現フレームのデータで置き換える preUserData.clear(); preUserData = userData; for (vector<UserData>::iterator p = preUserData.begin(); p != preUserData.end(); p++) { p->isDataFound = false; // 初期化 } }
void MultiCursorAppCpp::detectHeadPosition(CvBlobs blobs) { // Initialize userData userData.clear(); // 前フレームのラベルを見て最も投票数の多かったラベルを前フレームの対応領域とする for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) { if (!preLabelMat.empty()) { // 前フレームのラベルを投票 vector<Point2i> checkLabel; checkLabel.push_back(Point2i(0, 0)); // init checkLabel; for (int y = it->second->miny; y < it->second->maxy; ++y) { for (int x = it->second->minx; x < it->second->maxx; ++x) { unsigned long preLabel = preLabelMat.at<unsigned long>(y, x); if (it->first == labelMat.at<unsigned long>(y, x) && preLabel != 0) { bool isFoundLabel = false; for (int i = 0; i < checkLabel.size(); ++i) { if (checkLabel[i].x == preLabel) { ++checkLabel[i].y; isFoundLabel = true; break; } } if (!isFoundLabel) { Point2i newPoint(preLabel, 1); // (labelID, # of label) checkLabel.push_back(newPoint); } } } } // 最も投票数の多いラベルを探す Point2i mainLabel(0, 0); for (int i = 0; i < checkLabel.size(); ++i) { if (checkLabel[i].y > mainLabel.y) { mainLabel.x = checkLabel[i].x; mainLabel.y = checkLabel[i].y; } } // 前フレームのデータと対応付けて現フレームのデータを作る UserData newUserData; newUserData.isDataFound = false; if (mainLabel.y > it->second->area / 2) // 半分以上を占める領域がないときは無視 { for (vector<UserData>::iterator p = preUserData.begin(); p != preUserData.end(); ++p) { if (p->labelID == mainLabel.x) { p->isDataFound = true; newUserData.isDataFound = true; newUserData.headInfo.height = p->headInfo.height; newUserData.headInfo.depthPoint.x = p->headInfo.depthPoint.x; newUserData.headInfo.depthPoint.y = p->headInfo.depthPoint.y; break; } } } newUserData.labelID = it->first; newUserData.centroid.x = it->second->centroid.x; newUserData.centroid.y = it->second->centroid.y; userData.push_back(newUserData); } else { UserData newUserData; newUserData.isDataFound = false; newUserData.labelID = it->first; newUserData.centroid.x = it->second->centroid.x; newUserData.centroid.y = it->second->centroid.y; userData.push_back(newUserData); } } // Update preLabel preLabelMat = labelMat; // Find the highest point of each user area USHORT* headHeights = new USHORT[blobs.size()]; Point2i* newHighestPositions = new Point2i[blobs.size()]; int blobID = 0; for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); it++) { headHeights[blobID] = 0; newHighestPositions[blobID].x = 0; newHighestPositions[blobID].y = 0; for (int y = it->second->miny; y <= it->second->maxy; y++) { for (int x = it->second->minx; x <= it->second->maxx; x++) { if (0 <= blobID && blobID < blobs.size()) { USHORT height = *heightMatrix.ptr<USHORT>(y, x); if (headHeights[blobID] < height && height < HEAD_HEIGHT_MAX) { headHeights[blobID] = height; newHighestPositions[blobID].x = x; newHighestPositions[blobID].y = y; } } } } // Debug: Show the highest point of each users // circle(userAreaMat, Point(newHighestPositions[blobID].x, newHighestPositions[blobID].y), 5, Scalar(255, 0, 255), 3); blobID++; } // Define users' head positions Point2i* newHeadPositions = new Point2i[blobs.size()]; INT* numHeadPoints = new INT[blobs.size()]; blobID = 0; for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it) { // Set the highest position as a base point of searching userData[blobID].headInfo.depthPoint.x = newHighestPositions[blobID].x; userData[blobID].headInfo.depthPoint.y = newHighestPositions[blobID].y; userData[blobID].headInfo.height = headHeights[blobID]; if (userData[blobID].isDataFound) { // Check distance between 2d positions in current frame and in preframe float distance = sqrt( pow(userData[blobID].headInfo.depthPoint.x - preUserData[blobID].headInfo.depthPoint.x, 2) + pow(userData[blobID].headInfo.depthPoint.y - preUserData[blobID].headInfo.depthPoint.y, 2) ); float distanceZ = abs(heightMatrix.at<float>(userData[blobID].headInfo.depthPoint.y, userData[blobID].headInfo.depthPoint.x) - preUserData[blobID].headInfo.height); //cout << distance << endl; // If the point is far from predata, just use pre-data / もし前回のフレームより大きく頭の位置がずれていたら前回の値を使う if (distance > 80.0f || distanceZ > 1000.0f) { userData[blobID].headInfo.height = preUserData[blobID].headInfo.height; userData[blobID].headInfo.depthPoint.x = preUserData[blobID].headInfo.depthPoint.x; userData[blobID].headInfo.depthPoint.y = preUserData[blobID].headInfo.depthPoint.y; } } // Estimate exact head positions (Get average) numHeadPoints[blobID] = 0; newHeadPositions[blobID].x = 0; newHeadPositions[blobID].y = 0; #if 1 int offset_head = 40; // ミーティングルーム用 #else int offset_head = 100; // 作業場用 #endif int minY = userData[blobID].headInfo.depthPoint.y - offset_head; if (minY < 0) minY = 0; int maxY = userData[blobID].headInfo.depthPoint.y + offset_head; if (maxY > kinectBasics.heightDepth) maxY = kinectBasics.heightDepth; int minX = userData[blobID].headInfo.depthPoint.x - offset_head; if (minX < 0) minX = 0; int maxX = userData[blobID].headInfo.depthPoint.x + offset_head; if (maxX > kinectBasics.widthDepth) maxX = kinectBasics.widthDepth; for (int y = minY; y <= maxY; y++) { for (int x = minX; x <= maxX; x++) { USHORT height = *heightMatrix.ptr<USHORT>(y, x); //cout << it->first << ", " << labelMat.at<unsigned long>(y, x) << endl; if ((userData[blobID].headInfo.height - HEAD_LENGTH) < height && height < HEAD_HEIGHT_MAX && it->first == labelMat.at<unsigned long>(y, x) // 同じblob内のみ探索 ) { newHeadPositions[blobID].x += x; newHeadPositions[blobID].y += y; numHeadPoints[blobID]++; } } } blobID++; } // Make avarage pixel value of each head positions the head positions of users for (int i = 0; i < blobs.size(); i++) { if (numHeadPoints[i] != 0) { // Calculate head position in 2D pixel userData[i].headInfo.depthPoint.x = newHeadPositions[i].x / numHeadPoints[i]; userData[i].headInfo.depthPoint.y = newHeadPositions[i].y / numHeadPoints[i]; } else { // 点が見つからなかった場合は最も高い点を頭にする userData[i].headInfo.depthPoint.x = newHighestPositions[i].x; userData[i].headInfo.depthPoint.y = newHighestPositions[i].y; } // Calculate head position in 3D point float* headPosition = point3fMatrix.ptr<float>(userData[i].headInfo.depthPoint.y, userData[i].headInfo.depthPoint.x); userData[i].headInfo.cameraPoint.x = headPosition[0]; userData[i].headInfo.cameraPoint.y = headPosition[1]; userData[i].headInfo.cameraPoint.z = headPosition[2] + 0.2; // Debug: Show the head point circle(userAreaMat, Point(userData[i].headInfo.depthPoint.x, userData[i].headInfo.depthPoint.y), 7, Scalar(255, 0, 0), 3); } }
int main() { CvTracks tracks; cvNamedWindow("red_object_tracking", CV_WINDOW_AUTOSIZE); CvCapture *capture = cvCaptureFromCAM(0); cvGrabFrame(capture); IplImage *img = cvRetrieveFrame(capture); CvSize imgSize = cvGetSize(img); IplImage *frame = cvCreateImage(imgSize, img->depth, img->nChannels); IplConvKernel* morphKernel = cvCreateStructuringElementEx(5, 5, 1, 1, CV_SHAPE_RECT, NULL); //unsigned int frameNumber = 0; unsigned int blobNumber = 0; bool quit = false; while (!quit&&cvGrabFrame(capture)) { IplImage *img = cvRetrieveFrame(capture); cvConvertScale(img, frame, 1, 0); IplImage *segmentated = cvCreateImage(imgSize, 8, 1); // Detecting red pixels: // (This is very slow, use direct access better...) for (unsigned int j=0; j<imgSize.height; j++) for (unsigned int i=0; i<imgSize.width; i++) { CvScalar c = cvGet2D(frame, j, i); double b = ((double)c.val[0])/255.; double g = ((double)c.val[1])/255.; double r = ((double)c.val[2])/255.; unsigned char f = 255*((r>0.2+g)&&(r>0.2+b)); cvSet2D(segmentated, j, i, CV_RGB(f, f, f)); } cvMorphologyEx(segmentated, segmentated, NULL, morphKernel, CV_MOP_OPEN, 1); //cvShowImage("segmentated", segmentated); IplImage *labelImg = cvCreateImage(cvGetSize(frame), IPL_DEPTH_LABEL, 1); CvBlobs blobs; unsigned int result = cvLabel(segmentated, labelImg, blobs); cvFilterByArea(blobs, 500, 1000000); cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX); cvUpdateTracks(blobs, tracks, 200., 5); cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX); cvShowImage("red_object_tracking", frame); /*std::stringstream filename; filename << "redobject_" << std::setw(5) << std::setfill('0') << frameNumber << ".png"; cvSaveImage(filename.str().c_str(), frame);*/ cvReleaseImage(&labelImg); cvReleaseImage(&segmentated); char k = cvWaitKey(10)&0xff; switch (k) { case 27: case 'q': case 'Q': quit = true; break; case 's': case 'S': for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it) { std::stringstream filename; filename << "redobject_blob_" << std::setw(5) << std::setfill('0') << blobNumber << ".png"; cvSaveImageBlob(filename.str().c_str(), img, it->second); blobNumber++; std::cout << filename.str() << " saved!" << std::endl; } break; } cvReleaseBlobs(blobs); //frameNumber++; } cvReleaseStructuringElement(&morphKernel); cvReleaseImage(&frame); cvDestroyWindow("red_object_tracking"); return 0; }