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;
}
