void find_convex_hull(struct ctx *ctx)
{
CvSeq *defects;
CvConvexityDefect *defect_array;
int i;
int x = 0, y = 0;
int dist = 0;
ctx->hull = NULL;
if (!ctx->contour)
return;
ctx->hull = cvConvexHull2(ctx->contour, ctx->hull_st, CV_CLOCKWISE, 0);
if (ctx->hull) {
/* Get convexity defects of contour w.r.t. the convex hull */
defects = cvConvexityDefects(ctx->contour, ctx->hull,
ctx->defects_st);
if (defects && defects->total) {
defect_array = calloc(defects->total,
sizeof(CvConvexityDefect));
cvCvtSeqToArray(defects, defect_array, CV_WHOLE_SEQ);
/* Average depth points to get hand center */
for (i = 0; i < defects->total && i < NUM_DEFECTS; i++) {
x += defect_array[i].depth_point->x;
y += defect_array[i].depth_point->y;
ctx->defects[i] = cvPoint(defect_array[i].depth_point->x,
defect_array[i].depth_point->y);
}
x /= defects->total;
y /= defects->total;
ctx->num_defects = defects->total;
ctx->hand_center = cvPoint(x, y);
/* Compute hand radius as mean of distances of
defects' depth point to hand center */
for (i = 0; i < defects->total; i++) {
int d = (x - defect_array[i].depth_point->x) *
(x - defect_array[i].depth_point->x) +
(y - defect_array[i].depth_point->y) *
(y - defect_array[i].depth_point->y);
dist += sqrt(d);
}
ctx->hand_radius = dist / defects->total;
free(defect_array);
}
}
}
void getconvexhull()
{
hull = cvConvexHull2( contours, 0, CV_CLOCKWISE, 0 );
pt0 = **CV_GET_SEQ_ELEM( CvPoint*, hull, hull->total - 1 );
for(int i = 0; i < hull->total; i++ )
{
pt = **CV_GET_SEQ_ELEM( CvPoint*, hull, i );
//printf("%d,%d\n",pt.x,pt.y);
cvLine( frame, pt0, pt, CV_RGB( 128, 128, 128 ),2,8,0);
pt0 = pt;
}
defect = cvConvexityDefects(contours,hull,defectstorage); //��M�ʳ�
for(int i=0;i<defect->total;i++)
{
CvConvexityDefect* d=(CvConvexityDefect*)cvGetSeqElem(defect,i);
// if(d->depth < 50)
// {
// p.x = d->start->x;
// p.y = d->start->y;
// cvCircle(frame,p,5,CV_RGB(255,255,255),-1,CV_AA,0);
// p.x = d->end->x;
// p.y = d->end->y;
// cvCircle(frame,p,5,CV_RGB(255,255,255),-1,CV_AA,0);
// }
if(d->depth > 10)
{
p.x = d->depth_point->x;
p.y = d->depth_point->y;
cvCircle(frame,p,5,CV_RGB(255,255,0),-1,CV_AA,0);
cvSeqPush(palm,&p);
}
}
//if(palm->total>1)
//{
// cvMinEnclosingCircle(palm,&mincirclecenter,&radius);
// cvRound(radius);
// mincirclecenter2.x = cvRound(mincirclecenter.x);
// mincirclecenter2.y = cvRound(mincirclecenter.y);
// cvCircle(frame,mincirclecenter2,cvRound(radius),CV_RGB(255,128,255),4,8,0);
// cvCircle(frame,mincirclecenter2,10,CV_RGB(255,128,255),4,8,0);
// palmcenter = cvMinAreaRect2(palm,0);
// center.x = cvRound(palmcenter.center.x);
// center.y = cvRound(palmcenter.center.y);
// cvEllipseBox(frame,palmcenter,CV_RGB(128,128,255),2,CV_AA,0);
// cvCircle(frame,center,10,CV_RGB(128,128,255),-1,8,0);
//}
}
//Wrapper de C a CPP
void HandDetection::findConvexityDefects(vector<Point>& contour, vector<int>& hull, vector<CvConvexityDefect>& convexDefects){
if(hull.size() > 0 && contour.size() > 0){
//Conversion de objetos CPP a C
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 );
//Empezamos con los contornos: los pasamos a un array de objetos Seq
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);
}
//Ahora con los puntos del poligono convexo
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);
//Inicializamos la salida
storage = cvCreateMemStorage(0);
defects = cvConvexityDefects(contourPoints, &hullMat, storage);
defectArray = (CvConvexityDefect*)malloc(sizeof(CvConvexityDefect)*defects->total);
cvCvtSeqToArray(defects, defectArray, CV_WHOLE_SEQ);
//Conversion de C a CPP
//float cutoff = lower - (lower - upper) * 0.3f;
for(int i = 0; i<defects->total; i++){
convexDefects.push_back(defectArray[i]);
}
//Liberar vectores con cvReleaseMemStorage
//No olvides hacer un free
//free(hullK);
cvReleaseMemStorage(&contourStr);
cvReleaseMemStorage(&strDefects);
cvReleaseMemStorage(&storage);
//Devolvemos el vector de puntos con los defectos de convexidad
//return defects;
}
//return defectArray;
}
void ConvexityClassifier::findConvexityDefects(vector<Point>& contour, vector<int>& hull, vector<Point>& 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);
//We calculate convexity defects
storage = cvCreateMemStorage(0);
defects = cvConvexityDefects(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++){
CvPoint ptf;
ptf.x = defectArray[i].depth_point->x;
ptf.y = defectArray[i].depth_point->y;
convexDefects.push_back(ptf);
}
//We release memory
cvReleaseMemStorage(&contourStr);
cvReleaseMemStorage(&strDefects);
cvReleaseMemStorage(&storage);
}
}
static CvSeq *
get_defects (guint16* depth,
guint width,
guint height,
guint start_x,
guint start_y,
guint start_z)
{
IplImage *img;
IplImage *image = NULL;
CvSeq *points = NULL;
CvSeq *contours = NULL;
CvMemStorage *g_storage, *hull_storage;
img = segment_hand (depth,
width,
height,
start_x,
start_y,
start_z);
if (img == NULL)
{
return NULL;
}
image = cvCreateImage(cvGetSize(img), 8, 1);
cvCopy(img, image, 0);
cvSmooth(image, img, CV_MEDIAN, 7, 0, 0, 0);
cvThreshold(img, image, 150, 255, CV_THRESH_OTSU);
g_storage = cvCreateMemStorage (0);
cvFindContours (image, g_storage, &contours, sizeof(CvContour),
CV_RETR_EXTERNAL,
CV_CHAIN_APPROX_SIMPLE,
cvPoint(0,0));
hull_storage = cvCreateMemStorage(0);
if (contours)
{
points = cvConvexHull2(contours, hull_storage, CV_CLOCKWISE, 0);
return cvConvexityDefects (contours, points, NULL);
}
return NULL;
}
// this should be replaced by c++ 2.0 api style code once available
vector<cv::Vec4i> convexityDefects(const vector<cv::Point>& contour) {
vector<int> hullIndices;
convexHull(Mat(contour), hullIndices, false, false);
vector<cv::Vec4i> convexityDefects;
if(hullIndices.size() > 0 && contour.size() > 0) {
CvMat contourMat = cvMat(1, contour.size(), CV_32SC2, (void*) &contour[0]);
CvMat hullMat = cvMat(1, hullIndices.size(), CV_32SC1, (void*) &hullIndices[0]);
CvMemStorage* storage = cvCreateMemStorage(0);
CvSeq* defects = cvConvexityDefects(&contourMat, &hullMat, storage);
for(int i = 0; i < defects->total; i++){
CvConvexityDefect* cur = (CvConvexityDefect*) cvGetSeqElem(defects, i);
cv::Vec4i defect;
defect[0] = cur->depth_point->x;
defect[1] = cur->depth_point->y;
defect[2] = (cur->start->x + cur->end->x) / 2;
defect[3] = (cur->start->y + cur->end->y) / 2;
convexityDefects.push_back(defect);
}
cvReleaseMemStorage(&storage);
}
return convexityDefects;
}
void detect(IplImage* img_8uc1,IplImage* img_8uc3) {
//cvNamedWindow( "aug", 1 );
//cvThreshold( img_8uc1, img_edge, 128, 255, CV_THRESH_BINARY );
CvMemStorage* storage = cvCreateMemStorage();
CvSeq* first_contour = NULL;
CvSeq* maxitem=NULL;
double area=0,areamax=0;
int maxn=0;
int Nc = cvFindContours(
img_8uc1,
storage,
&first_contour,
sizeof(CvContour),
CV_RETR_LIST // Try all four values and see what happens
);
int n=0;
//printf( "Total Contours Detected: %d\n", Nc );
if(Nc>0)
{
for( CvSeq* c=first_contour; c!=NULL; c=c->h_next )
{
//cvCvtColor( img_8uc1, img_8uc3, CV_GRAY2BGR );
area=cvContourArea(c,CV_WHOLE_SEQ );
if(area>areamax)
{areamax=area;
maxitem=c;
maxn=n;
}
n++;
}
CvMemStorage* storage3 = cvCreateMemStorage(0);
//if (maxitem) maxitem = cvApproxPoly( maxitem, sizeof(maxitem), storage3, CV_POLY_APPROX_DP, 3, 1 );
if(areamax>5000)
{
maxitem = cvApproxPoly( maxitem, sizeof(CvContour), storage3, CV_POLY_APPROX_DP, 10, 1 );
CvPoint pt0;
CvMemStorage* storage1 = cvCreateMemStorage(0);
CvMemStorage* storage2 = cvCreateMemStorage(0);
CvSeq* ptseq = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvContour),
sizeof(CvPoint), storage1 );
CvSeq* hull;
CvSeq* defects;
for(int i = 0; i < maxitem->total; i++ )
{ CvPoint* p = CV_GET_SEQ_ELEM( CvPoint, maxitem, i );
pt0.x = p->x;
pt0.y = p->y;
cvSeqPush( ptseq, &pt0 );
}
hull = cvConvexHull2( ptseq, 0, CV_CLOCKWISE, 0 );
int hullcount = hull->total;
defects= cvConvexityDefects(ptseq,hull,storage2 );
//printf(" defect no %d \n",defects->total);
CvConvexityDefect* defectArray;
int j=0;
//int m_nomdef=0;
// This cycle marks all defects of convexity of current contours.
for(;defects;defects = defects->h_next)
{
int nomdef = defects->total; // defect amount
//outlet_float( m_nomdef, nomdef );
//printf(" defect no %d \n",nomdef);
if(nomdef == 0)
continue;
// Alloc memory for defect set.
//fprintf(stderr,"malloc\n");
defectArray = (CvConvexityDefect*)malloc(sizeof(CvConvexityDefect)*nomdef);
// Get defect set.
//fprintf(stderr,"cvCvtSeqToArray\n");
cvCvtSeqToArray(defects,defectArray, CV_WHOLE_SEQ);
// Draw marks for all defects.
for(int i=0; i<nomdef; i++)
{ printf(" defect depth for defect %d %f \n",i,defectArray[i].depth);
cvLine(img_8uc3, *(defectArray[i].start), *(defectArray[i].depth_point),CV_RGB(255,255,0),1, CV_AA, 0 );
cvCircle( img_8uc3, *(defectArray[i].depth_point), 5, CV_RGB(0,0,164), 2, 8,0);
cvCircle( img_8uc3, *(defectArray[i].start), 5, CV_RGB(0,0,164), 2, 8,0);
cvLine(img_8uc3, *(defectArray[i].depth_point), *(defectArray[i].end),CV_RGB(255,255,0),1, CV_AA, 0 );
}
char txt[]="0";
txt[0]='0'+nomdef-1;
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 5, CV_AA);
cvPutText(img_8uc3, txt, cvPoint(50, 50), &font, cvScalar(0, 0, 255, 0));
j++;
// Free memory.
free(defectArray);
}
cvReleaseMemStorage( &storage );
cvReleaseMemStorage( &storage1 );
cvReleaseMemStorage( &storage2 );
cvReleaseMemStorage( &storage3 );
//return 0;
}
}
}
void detect(IplImage* img_8uc1,IplImage* img_8uc3)
{
clock_t inicio, fin;
inicio = clock();
CvMemStorage* storage = cvCreateMemStorage();
CvSeq* first_contour = NULL;
CvSeq* maxitem=NULL;
char resultado [] = " ";
double area=0,areamax=0;
double longitudExt = 0;
double radio = 0;
int maxn=0;
int Nc = cvFindContours(
img_8uc1,
storage,
&first_contour,
sizeof(CvContour),
CV_RETR_LIST
);
int n=0;
//printf( "Contornos detectados: %d\n", Nc );
if(Nc>0)
{
for( CvSeq* c=first_contour; c!=NULL; c=c->h_next )
{
area=cvContourArea(c,CV_WHOLE_SEQ );
if(area>areamax)
{
areamax=area;
maxitem=c;
maxn=n;
}
n++;
}
CvMemStorage* storage3 = cvCreateMemStorage(0);
if(areamax>5000)
{
maxitem = cvApproxPoly( maxitem, sizeof(CvContour), storage3, CV_POLY_APPROX_DP, 10, 1 );
CvPoint pt0;
CvMemStorage* storage1 = cvCreateMemStorage(0);
CvMemStorage* storage2 = cvCreateMemStorage(0);
CvSeq* ptseq = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvContour), sizeof(CvPoint), storage1 );
CvSeq* hull;
CvSeq* defects;
CvPoint minDefectPos;;
minDefectPos.x = 1000000;
minDefectPos.y = 1000000;
CvPoint maxDefectPos;
maxDefectPos.x = 0;
maxDefectPos.y = 0;
for(int i = 0; i < maxitem->total; i++ )
{
CvPoint* p = CV_GET_SEQ_ELEM( CvPoint, maxitem, i );
pt0.x = p->x;
pt0.y = p->y;
cvSeqPush( ptseq, &pt0 );
}
hull = cvConvexHull2( ptseq, 0, CV_CLOCKWISE, 0 );
int hullcount = hull->total;
defects= cvConvexityDefects(ptseq,hull,storage2 );
//printf(" Numero de defectos %d \n",defects->total);
CvConvexityDefect* defectArray;
int j=0;
// This cycle marks all defects of convexity of current contours.
longitudExt = 0;
for(;defects;defects = defects->h_next)
{
int nomdef = defects->total; // defect amount
//outlet_float( m_nomdef, nomdef );
//printf(" defect no %d \n",nomdef);
if(nomdef == 0)
continue;
// Alloc memory for defect set.
//fprintf(stderr,"malloc\n");
defectArray = (CvConvexityDefect*)malloc(sizeof(CvConvexityDefect)*nomdef);
// Get defect set.
//fprintf(stderr,"cvCvtSeqToArray\n");
cvCvtSeqToArray(defects,defectArray, CV_WHOLE_SEQ);
// Draw marks for all defects.
for(int i=0; i<nomdef; i++)
{
CvPoint startP;
startP.x = defectArray[i].start->x;
startP.y = defectArray[i].start->y;
CvPoint depthP;
depthP.x = defectArray[i].depth_point->x;
depthP.y = defectArray[i].depth_point->y;
CvPoint endP;
endP.x = defectArray[i].end->x;
endP.y = defectArray[i].end->y;
//obtener minimo y maximo
minDefectPos.x = getMin (startP.x, depthP.x, endP.x, minDefectPos.x);
minDefectPos.y = getMin (startP.y, depthP.y, endP.y, minDefectPos.y);
maxDefectPos.x = getMax (startP.x, depthP.x, endP.x, maxDefectPos.x);
maxDefectPos.y = getMax (startP.y, depthP.y, endP.y, maxDefectPos.y);
//fin obtener minimo y maximo
if (saveLength)
{
longitudExt += longBtwnPoints(startP, depthP);
longitudExt += longBtwnPoints(depthP, endP);
}
//printf(" defect depth for defect %d %f \n",i,defectArray[i].depth);
cvLine(img_8uc3, startP, depthP, CV_RGB(255,255,0),1, CV_AA, 0 );
cvCircle( img_8uc3, depthP, 5, CV_RGB(0,0,164), 2, 8,0);
cvCircle( img_8uc3, startP, 5, CV_RGB(255,0,0), 2, 8,0);
cvCircle( img_8uc3, endP, 5, CV_RGB(0,255,0), 2, 8,0);
cvLine(img_8uc3, depthP, endP,CV_RGB(0,0,0),1, CV_AA, 0 );
}
/*if (nomdef>0)
{
resultado [0] = identificaGesto (longitudExt, nomdef, radio);
if (resultado[0] !=' ')
printf ("Gesto identificado (%c) \n", resultado[0]);
}*/
if (saveLength)
{
radio = (double)maxDefectPos.x / (double)maxDefectPos.y;
if (nomdef>0)
{
printf ("_______________________\n");
resultado [0] = identificaGesto (longitudExt, nomdef, radio);
fin = clock();
fin = fin - inicio;
if (resultado[0] !=' ')
printf ("Gesto identificado (%c) \n", resultado[0]);
else
printf ("No se identifico ningun gesto\n");
printf("Tiempo de ejecucion: %f\nLongitud %g \nNomDef %i \nradio %g \n",(((float)fin)/CLOCKS_PER_SEC ), longitudExt, nomdef, radio);
FILE *fp;
fp=fopen("archivo.txt", "a");
if (nomdef == 6)
fprintf(fp, "\n>>>>>>>5<<<<<<\n%g\n%i\n%g\n",longitudExt, nomdef, radio);
else
fprintf(fp, "\n%g\n%i\n%g\n",longitudExt, nomdef, radio);
fclose (fp);
}
else
printf("No hay defectos");
printf ("_______________________\n");
}
/*
char txt[]="0";
txt[0]='0'+nomdef-1;
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 5, CV_AA);
cvPutText(img_8uc3, txt, cvPoint(50, 50), &font, cvScalar(0, 0, 255, 0)); */
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 5, CV_AA);
if (resultado!= NULL)
cvPutText(img_8uc3, resultado, cvPoint(50, 50), &font, cvScalar(0, 0, 255, 0));
j++;
// Free memory.
free(defectArray);
}
pt0 = **CV_GET_SEQ_ELEM( CvPoint*, hull, hullcount - 1 );
for(int i = 0; i < hullcount; i++ )
{
CvPoint pt = **CV_GET_SEQ_ELEM( CvPoint*, hull, i );
cvLine( img_8uc3, pt0, pt, CV_RGB( 0, 255, 0 ), 1, CV_AA, 0 );
pt0 = pt;
}
cvLine( img_8uc3, minDefectPos, cvPoint( (maxDefectPos.x), (minDefectPos.y)), CV_RGB( 2500, 0, 0 ), 1, CV_AA, 0 );
cvLine( img_8uc3, cvPoint( (maxDefectPos.x), (minDefectPos.y)), maxDefectPos, CV_RGB( 2500, 0, 0 ), 1, CV_AA, 0 );
cvLine( img_8uc3, maxDefectPos, cvPoint( (minDefectPos.x), (maxDefectPos.y)), CV_RGB( 2500, 0, 0 ), 1, CV_AA, 0 );
cvLine( img_8uc3, cvPoint( (minDefectPos.x), (maxDefectPos.y)), minDefectPos, CV_RGB( 2500, 0, 0 ), 1, CV_AA, 0 );
cvReleaseMemStorage( &storage );
cvReleaseMemStorage( &storage1 );
cvReleaseMemStorage( &storage2 );
cvReleaseMemStorage( &storage3 );
//return 0;
}
}
void AirCursor::analyzeGrab()
{
cvClearMemStorage(m_cvMemStorage);
// get current depth map from Kinect
const XnDepthPixel* depthMap = m_depthGenerator.GetDepthMap();
// convert 16bit openNI depth map to 8bit IplImage used in opencv processing
int origDepthIndex = 0;
char* depthPtr = m_iplDepthMap->imageData;
char* debugPtr = 0;
if (m_debugImageEnabled) debugPtr = m_iplDebugImage->imageData;
for (unsigned int y = 0; y < DEPTH_MAP_SIZE_Y; y++)
{
for (unsigned int x = 0; x < DEPTH_MAP_SIZE_X; x++)
{
// get current depth value from original depth map
short depth = depthMap[origDepthIndex];
// check that current value is in the allowed range determined by clipping distances,
// and if it is map it to range 0 - 255 so that 255 is the closest value
unsigned char pixel = 0;
if (depth >= NEAR_CLIPPING_DISTANCE && depth <= FAR_CLIPPING_DISTANCE)
{
depth -= NEAR_CLIPPING_DISTANCE;
pixel = 255 - (255.0f * ((float)depth / (FAR_CLIPPING_DISTANCE - NEAR_CLIPPING_DISTANCE)));
}
else {
pixel = 0;
}
m_iplDepthMap->imageData[y * m_iplDepthMap->widthStep + x] = pixel;
*depthPtr = pixel;
if (m_debugImageEnabled)
{
// init debug image with the same depth map
*(debugPtr + 0) = pixel;
*(debugPtr + 1) = pixel;
*(debugPtr + 2) = pixel;
debugPtr += 3;
}
origDepthIndex++;
depthPtr++;
}
}
// calculate region of interest corner points in real world coordinates
XnPoint3D rwPoint1 = m_handPosRealWorld;
rwPoint1.X -= HAND_ROI_SIZE_LEFT;
rwPoint1.Y += HAND_ROI_SIZE_UP;
XnPoint3D rwPoint2 = m_handPosRealWorld;
rwPoint2.X += HAND_ROI_SIZE_RIGHT;
rwPoint2.Y -= HAND_ROI_SIZE_DOWN;
// convert corner points to projective coordinates
XnPoint3D projPoint1, projPoint2;
m_depthGenerator.ConvertRealWorldToProjective(1, &rwPoint1, &projPoint1);
m_depthGenerator.ConvertRealWorldToProjective(1, &rwPoint2, &projPoint2);
// round projected corner points to ints and clip them against the depth map
int ROItopLeftX = qRound(projPoint1.X); int ROItopLeftY = qRound(projPoint1.Y);
int ROIbottomRightX = qRound(projPoint2.X); int ROIbottomRightY = qRound(projPoint2.Y);
if (ROItopLeftX < 0) ROItopLeftX = 0; else if (ROItopLeftX > DEPTH_MAP_SIZE_X - 1) ROItopLeftX = DEPTH_MAP_SIZE_X - 1;
if (ROItopLeftY < 0) ROItopLeftY = 0; else if (ROItopLeftY > DEPTH_MAP_SIZE_Y - 1) ROItopLeftY = DEPTH_MAP_SIZE_Y - 1;
if (ROIbottomRightX < 0) ROIbottomRightX = 0; else if (ROIbottomRightX > DEPTH_MAP_SIZE_X - 1) ROIbottomRightX = DEPTH_MAP_SIZE_X - 1;
if (ROIbottomRightY < 0) ROIbottomRightY = 0; else if (ROIbottomRightY > DEPTH_MAP_SIZE_Y - 1) ROIbottomRightY = DEPTH_MAP_SIZE_Y - 1;
// set region of interest
CvRect rect = cvRect(ROItopLeftX, ROItopLeftY, ROIbottomRightX - ROItopLeftX, ROIbottomRightY - ROItopLeftY);
if(rect.height > 0 && rect.width > 0)
{
cvSetImageROI(m_iplDepthMap, rect);
if (m_debugImageEnabled) cvSetImageROI(m_iplDebugImage, rect);
}
// use depth threshold to isolate hand
// as a center point of thresholding, it seems that it's better to use a point bit below
// the point Nite gives as the hand point
XnPoint3D rwThresholdPoint = m_handPosRealWorld; rwThresholdPoint.Y -= 30;
XnPoint3D projThresholdPoint;
m_depthGenerator.ConvertRealWorldToProjective(1, &rwThresholdPoint, &projThresholdPoint);
int lowerBound = (unsigned char)m_iplDepthMap->imageData[(int)projThresholdPoint.Y * DEPTH_MAP_SIZE_X + (int)projThresholdPoint.X] - DEPTH_THRESHOLD;
if (lowerBound < 0) lowerBound = 0;
cvThreshold( m_iplDepthMap, m_iplDepthMap, lowerBound, 255, CV_THRESH_BINARY );
// color used for drawing the hand in the debug image, green for normal and red for grab.
// color lags one frame from actual grab status but in practice that shouldn't be too big of a problem
int rCol, gCol, bCol;
if(m_grabbing) {
rCol = 255; gCol = 0; bCol = 0;
}
else {
rCol = 0; gCol = 255; bCol = 0;
}
// go through the ROI and paint hand on debug image with current grab status color
if (m_debugImageEnabled)
{
// index of first pixel in the ROI
int startIndex = ROItopLeftY * m_iplDepthMap->widthStep + ROItopLeftX;
depthPtr = &(m_iplDepthMap->imageData[startIndex]);
debugPtr = &(m_iplDebugImage->imageData[startIndex * 3]);
// how much index needs to increase when moving to next line
int vertInc = m_iplDepthMap->widthStep - (ROIbottomRightX - ROItopLeftX);
for (int y = ROItopLeftY; y < ROIbottomRightY; y++)
{
for (int x = ROItopLeftX; x < ROIbottomRightX; x++)
{
if((unsigned char)*depthPtr > 0)
{
*(debugPtr + 0) = rCol / 2;
*(debugPtr + 1) = gCol / 2;
*(debugPtr + 2) = bCol / 2;
}
// next pixel
depthPtr++;
debugPtr += 3;
}
// next line
depthPtr += vertInc;
debugPtr += vertInc * 3;
}
}
// find contours in the hand and draw them on debug image
CvSeq* contours = 0;
cvFindContours(m_iplDepthMap, m_cvMemStorage, &contours, sizeof(CvContour));
if (m_debugImageEnabled)
{
if(contours)
{
cvDrawContours(m_iplDebugImage, contours, cvScalar(rCol, gCol , bCol), cvScalar(rCol, gCol, bCol), 1);
}
}
// go through contours and search for the biggest one
CvSeq* biggestContour = 0;
double biggestArea = 0.0f;
for(CvSeq* currCont = contours; currCont != 0; currCont = currCont->h_next)
{
// ignore small contours which are most likely caused by artifacts
double currArea = cvContourArea(currCont);
if(currArea < CONTOUR_MIN_SIZE) continue;
if(!biggestContour || currArea > biggestArea) {
biggestContour = currCont;
biggestArea = currArea;
}
}
int numOfValidDefects = 0;
if(biggestContour)
{
// calculate convex hull of the biggest contour found which is hopefully the hand
CvSeq* hulls = cvConvexHull2(biggestContour, m_cvMemStorage, CV_CLOCKWISE, 0);
if (m_debugImageEnabled)
{
// calculate convex hull and return it in a different form.
// only required for drawing
CvSeq* hulls2 = cvConvexHull2(biggestContour, m_cvMemStorage, CV_CLOCKWISE, 1);
// draw the convex hull
cvDrawContours(m_iplDebugImage, hulls2, cvScalar(rCol, gCol , bCol), cvScalar(rCol, gCol, bCol), 1);
}
// calculate convexity defects of hand's convex hull
CvSeq* defects = cvConvexityDefects(biggestContour, hulls, m_cvMemStorage);
int numOfDefects = defects->total;
if (numOfDefects > 0)
{
// calculate defect min size in projective coordinates.
// this is done using a vector from current hand position to a point DEFECT_MIN_SIZE amount above it.
// that vector is converted to projective coordinates and it's length is calculated.
XnPoint3D rwTempPoint = m_handPosRealWorld;
rwTempPoint.Y += DEFECT_MIN_SIZE;
XnPoint3D projTempPoint;
m_depthGenerator.ConvertRealWorldToProjective(1, &rwTempPoint, &projTempPoint);
int defectMinSizeProj = m_handPosProjected.Y - projTempPoint.Y;
// convert opencv seq to array
CvConvexityDefect* defectArray;defectArray = (CvConvexityDefect*)malloc(sizeof(CvConvexityDefect) * numOfDefects);
cvCvtSeqToArray(defects, defectArray, CV_WHOLE_SEQ);
for(int i = 0; i < numOfDefects; i++)
{
// ignore too small defects
if((defectArray[i].depth) < defectMinSizeProj)
{
continue;
}
numOfValidDefects++;
if (m_debugImageEnabled)
{
// draw blue point to defect
cvCircle(m_iplDebugImage, *(defectArray[i].depth_point), 5, cvScalar(0, 0, 255), -1);
cvCircle(m_iplDebugImage, *(defectArray[i].start), 5, cvScalar(0, 0, 255), -1);
cvCircle(m_iplDebugImage, *(defectArray[i].end), 5, cvScalar(0, 0, 255), -1);
}
}
free(defectArray);
}
}
if (m_debugImageEnabled)
{
cvResetImageROI(m_iplDebugImage);
// draw white dot on current hand position
cvCircle(m_iplDebugImage, cvPoint(m_handPosProjected.X, m_handPosProjected.Y), 5, cvScalar(255, 255, 255), -1);
// draw gray dot on current center of threshold position
//cvCircle(m_iplDebugImage, cvPoint(projThresholdPoint.X, projThresholdPoint.Y), 5, cvScalar(127, 127, 127), -1);
// draw ROI with green
//cvRectangle(m_iplDebugImage, cvPoint(ROItopLeftX, ROItopLeftY), cvPoint(ROIbottomRightX, ROIbottomRightY), cvScalar(0, 255, 0));
}
// determine current grab status based on defect count
if(numOfValidDefects <= GRAB_MAX_DEFECTS)
{
m_currentGrab = true;
}
else
{
m_currentGrab = false;
}
if (m_debugImageEnabled)
{
// debug strings
QList<QString> debugStrings;
debugStrings.push_back(QString("hand distance: " + QString::number(m_handPosRealWorld.Z) + " mm").toStdString().c_str());
debugStrings.push_back(QString("defects: " + QString::number(numOfValidDefects)).toStdString().c_str());
// convert iplDebugImage to QImage
char* scanLinePtr = m_iplDebugImage->imageData;
for (int y = 0;y < DEPTH_MAP_SIZE_Y; y++) {
memcpy(m_debugImage->scanLine(y), scanLinePtr, DEPTH_MAP_SIZE_X * 3);
scanLinePtr += DEPTH_MAP_SIZE_X * 3;
}
emit debugUpdate(*m_debugImage, debugStrings);
}
}
