Permalink
Switch branches/tags
Nothing to show
Find file
Fetching contributors…
Cannot retrieve contributors at this time
317 lines (248 sloc) 7.25 KB
/* Posture detection using blob analysis
To the glory of Yeshua Ha'Mashiach
Idris Soule, Michael Pang
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <conio.h>
#include <math.h>
#include <float.h>
#include <errno.h>
#include <highgui.h>
#include <pthread.h>
#include <signal.h>
#include <windows.h>
#include "fsm.h"
#include "Blob.h"
#include "BlobResult.h"
#include "NPTrackingTools.h"
#if OCV_DEBUG
#include "ocv.h"
#endif
#define W 200
#define H 200
#define KEY_ESC 27
#define KEY_NOTPRESSED 0
#define MAX_NUM_CAMERAS 3
#pragma warning(disable:4716) //disable missing return from function error
int key = KEY_NOTPRESSED;
pthread_mutex_t keyMutex = PTHREAD_ERRORCHECK_MUTEX_INITIALIZER;
typedef struct {
unsigned int i;
IplImage *displayImage;
}CameraData_t;
int sortLowHigh(const void * a, const void *b)
{
return (int)(*(double*)a - *(double*)b);
}
/* Filter to retrieve modifiers in the track state
** These modifiers are for left and right click
*/
class ModifierFilter {
public:
static const int MAX_BLOBS = 10;
ModifierFilter() {
bres = new CBlobResult();
};
~ModifierFilter() {
if(bres)
delete bres;
}
CBlobResult * modifiers(const CBlobResult *cbr);
CBlob keyFeature(const CBlobResult *cbr);
private:
CBlobResult *bres;
CBlob kf;
double m1[10], m2[10], m3[10];
double m4[10];
};
/* ModifierFilter::modifiers
Returns the two modifiers left, right
@cbr: a CBlobResult of the current hand
@return: the two filtered modifiers
*/
CBlobResult * ModifierFilter::modifiers(const CBlobResult *cbr)
{
const int numBlobs = cbr->GetNumBlobs();
assert(numBlobs <= MAX_BLOBS);
for(int i = 0; i < numBlobs; i++){
CBlob blob = cbr->GetBlob(i);
if(blob.MaxY() >= W) //blobs of image width/height
m1[i] = W;
else
m1[i] = cbr->GetBlob(i).MinX();
}
qsort(m1, numBlobs, sizeof(double), &sortLowHigh);
for(int j = 0; j < numBlobs; j++)
{
CBlob blob = cbr->GetBlob(j);
if(blob.MaxY() >= W) //blobs of image width/height
m2[j] = W;
else
m2[j] = cbr->GetBlob(j).MaxX();
}
qsort(m2, numBlobs, sizeof(double), &sortLowHigh);
for(int k = 0; k < numBlobs; k++) //RC sphere
{
CBlob blob = cbr->GetBlob(k);
if(blob.MaxY() >= W) //blobs of image width/height
m3[k] = W;
else
m3[k] = cbr->GetBlob(k).MinY();
}
qsort(m3, numBlobs, sizeof(double), &sortLowHigh);
//check blobs against m1, m2, m3
for(int i = 0; i < numBlobs; i++)
{
CBlob b = cbr->GetBlob(i);
if(m1[0] == b.MinX() && m2[0] == b.MaxX())
bres->AddBlob(&cbr->GetBlob(i));
if(m3[0] == b.MinY())
bres->AddBlob(&cbr->GetBlob(i));
}
return bres;
}
/*
Modifier::keyFeature
Calculate keyfeature
*/
CBlob ModifierFilter::keyFeature(const CBlobResult *cbr)
{
assert(cbr);
const int numBlobs = cbr->GetNumBlobs();
assert(numBlobs <= MAX_BLOBS);
for(int i = 0; i < numBlobs; i++){
CBlob blob = cbr->GetBlob(i);
if(blob.MaxY() >= W) //blobs of image width/height
m4[i] = -W;
else
m4[i] = cbr->GetBlob(i).MaxY();
}
qsort(m4, numBlobs, sizeof(double), &sortLowHigh);
for(int i = 0; i < numBlobs; i++)
{
CBlob b = cbr->GetBlob(i);
if(b.MaxY() == m4[numBlobs - 1]){
kf = cbr->GetBlob(i) ;
}
}
return kf;
}
/* thread to execute display of camera frames */
void *showCameraWindow(void *arg)
{
CameraData_t *myCam = (CameraData_t *)arg;
const char *windowName = TT_CameraName(myCam->i);
const CBlobResult *hand;
int x = 0, y = 0;
int lx = 0, ly = 0;
int rx = 0, ry = 0;
enum {EMPTY = 2, DRAG = 4, _ZOOM = 8, _TRACK = 9};
cvNamedWindow(windowName,CV_WINDOW_AUTOSIZE);
if(myCam->i != 0)
pthread_exit(NULL); //use Camera 21 for now
for( ;key != KEY_ESC; ){
TT_CameraFrameBuffer(myCam->i, W, H, 0, 8, (unsigned char *)myCam->displayImage->imageData);
cvFlip(myCam->displayImage, 0, -1);
cvShowImage(windowName, myCam->displayImage);
pthread_mutex_lock(&keyMutex);
hand = new CBlobResult(myCam->displayImage, 0, 20, false);
switch(hand->GetNumBlobs()){
case EMPTY:
puts("Hand not in view!");
break;
case _ZOOM: puts("ZOOM");
break;
case _TRACK:
do {
ModifierFilter modFilter;
CBlobResult *filteredHand = modFilter.modifiers(hand);
CBlob keyF = modFilter.keyFeature(hand);
#if 1
POINT cursor;
CBlobGetXCenter centreX;
CBlobGetYCenter centreY;
cursor.x = (LONG) centreX(keyF);
cursor.y = (LONG) centreY(keyF);
#endif
if( x >= cursor.x - 2 && x <= cursor.x + 2 &&
y >= cursor.y - 2 && y <= cursor.y + 2) {//some type of click?
POINT lSphere, rSphere;
lSphere.x = (LONG) centreX(filteredHand->GetBlob(1)); //TODO Assert idx 1
lSphere.y = (LONG) centreY(filteredHand->GetBlob(1));
rSphere.x = (LONG) centreX(filteredHand->GetBlob(0));
rSphere.y = (LONG) centreY(filteredHand->GetBlob(0));
const int threshold = 3;
if( ly >= (lSphere.y - threshold) && ly <= (lSphere.y + threshold)){ //LEFT CLICK
printf("LEFT CLICK \n");
lx = lSphere.x;
ly = lSphere.y;
}
}
else { //purely tracking as keyfeature has changed
printf("-- true tracking\n");
x = cursor.x;
y = cursor.y;
}
}
while(0);
break;
default:
puts("<<UNKNOWN>>\n");
}
delete hand;
key = cvWaitKey(50); //delay atleast n ms for TT firmware propogation delay
pthread_mutex_unlock(&keyMutex);
}
cvReleaseImage(&myCam->displayImage);
pthread_exit(NULL);
}
int main()
{
TT_Initialize(); //setup TT cameras
printf("Opening Calibration: %s\n",
TT_LoadCalibration("CalibrationResult 2010-12-30 4.39pm.cal") == NPRESULT_SUCCESS ?
"PASS" : "ERROR");
int cameraCount = TT_CameraCount();
CameraData_t cameras[MAX_NUM_CAMERAS];
pthread_t threads[MAX_NUM_CAMERAS];
assert(MAX_NUM_CAMERAS == cameraCount);
TT_SetCameraSettings(0, NPVIDEOTYPE_PRECISION,300, 150, 15);
TT_SetCameraSettings(1, NPVIDEOTYPE_PRECISION,300, 150, 15);
TT_SetCameraSettings(2, NPVIDEOTYPE_PRECISION,300, 150, 15);
/* 1. Change camera settings ^
2. Allocate space for the displays
*/
for(int i = 0; i < cameraCount; i++){
cameras[i].i = i;
cameras[i].displayImage = cvCreateImage(cvSize(W,H), IPL_DEPTH_8U, 1);
}
/* Setup priority for the process, pthreads-win32 threads inherit process priority
threads don't support RT or near-RT threads natively
*/
SetPriorityClass(GetCurrentProcess(), NORMAL_PRIORITY_CLASS/*(HIGH)REALTIME_PRIORITY_CLASS*/);
fsm_initialize(TRACK);
for(int i = 0; i < cameraCount; i++){
if(pthread_create(&threads[i], NULL, showCameraWindow, (void*)&cameras[i])){
printf("\aThread couldn't be created!");
cvDestroyAllWindows();
TT_Shutdown();
TT_FinalCleanup();
exit(-1);
}
}
printf("Press any Key to Exit!\n");
while(!_kbhit()){
int result = TT_Update();
if(result != NPRESULT_SUCCESS)
Sleep(10UL); //wait for updated frame 1/sleeptime[ms] = frame-rate
}
for(int i = 0; i < cameraCount; i++)
pthread_join(threads[i], NULL);
pthread_mutex_destroy(&keyMutex);
cvDestroyAllWindows();
TT_Shutdown();
TT_FinalCleanup();
return 0;
}