view src/testApp.cpp @ 2:c265c6b3b98d

Added bin/data/

#include "testApp.h"

#define IMGWIDTH  640
#define IMGHEIGHT 480

static int Colors[6][3] = {
	255,   0,   0,
	  0, 200,   0,
	  0,   0, 255,
	200, 175,   0,
	  0, 200, 255,
	255,   0, 255
};


/* Not using these for the time being
// Callback: A user was found
void XN_CALLBACK_TYPE NewUserDetected(UserGenerator& rGenerator,XnUserID nID ,void* pCookie) {
	printf("New User %d\n", nID);
	send_osc_i(((testApp*)pCookie)->sender),"/birth",nID);
}

// Callback: An existing user was lost
void XN_CALLBACK_TYPE UserLostDetected(UserGenerator& rGenerator ,XnUserID nID ,void* pCookie) {
	printf("Lost user %d\n", nID);
	send_osc_i(((testApp*)pCookie)->sender),"/death",nID);
}
*/

static void inline send_osc_i(ofxOscSender& sender, const char *path, int arg) {
	ofxOscMessage msg;
	msg.setAddress(path);
	msg.addIntArg(arg);
	sender.sendMessage(msg);
}

static inline void check_rc(XnStatus rc, const char *tag){
	if (rc != XN_STATUS_OK) {
		printf("FAILURE: %s - %s\n",tag,xnGetStatusString(rc));
		OF_EXIT_APP(0);
	}
}

static inline int check_nonfatal(XnStatus rc, const char *tag){
	if (rc != XN_STATUS_OK) {
		printf("WARNING: %s - %s\n",tag,xnGetStatusString(rc));
		return 1;
	} else return 0;
}

//--------------------------------------------------------------

class int3d { 
public:
	int X, Y, Z; 
	inline void set(int x, int y, int z) { X=x; Y=y; Z=z; }
	inline void add(int x, int y, int z) { X+=x; Y+=y; Z+=z; }
	inline void div(float d, XnVector3D *p) { p->X=X/d; p->Y=Y/d; p->Z=Z/d; }
	inline void cpy(XnVector3D *p) { p->X=X; p->Y=Y; p->Z=Z; }
};

static void project_into_plane(XnPlane3D& plane, XnVector3D& r, XnVector3D *q) {
	float l = (r.X-plane.ptPoint.X)*plane.vNormal.X 
			+ (r.Y-plane.ptPoint.Y)*plane.vNormal.Y
			+ (r.Z-plane.ptPoint.Z)*plane.vNormal.Z;
	
	q->X = r.X - l*plane.vNormal.X;
	q->Y = r.Y - l*plane.vNormal.Y;
	q->Z = r.Z - l*plane.vNormal.Z;
}

class UserStats {
public:
	struct int3d left, right, top, bottom, sum;
	int numPixels;
	
	UserStats() { 
		left.X=IMGWIDTH; 
		top.Y=IMGHEIGHT; 
		right.X=bottom.Y=0;
		sum.X=sum.Y=sum.Z=0;
		numPixels=0;
	}

	inline int  area() { return numPixels; }
	inline void centroid(XnVector3D *p) { sum.div(numPixels,p); }
	inline void accum(int x, int y, int z) {
		if (x<left.X)   left.set(x,y,z);
		if (x>right.X)  right.set(x,y,z);
		if (y<top.Y)    top.set(x,y,z);
		if (y>bottom.Y) bottom.set(x,y,z);
		sum.add(x,y,z);
		numPixels++;
	}
	
};


//--------------------------------------------------------------

testApp::testApp(const char *host, int port, int FPS) {
	ofSetupOpenGL(&window, IMGWIDTH,IMGHEIGHT, OF_WINDOW);
	ofSetWindowTitle("Idyom Installation Tracker");
	ofSetFrameRate(FPS);

	// initialise OSC sender
	printf("OSC target: %s:%d.\n",host,port);
	printf("Will run at %d frames per second.\n",FPS);
	sender.setup(host, port);
	draw_image=2;
}

static XnMapOutputMode fps_map_mode(int fps) {
	XnMapOutputMode mapMode;
	
	mapMode.nXRes = IMGWIDTH;
	mapMode.nYRes = IMGHEIGHT;
	mapMode.nFPS  = fps;
	return mapMode;
}
	
void testApp::setup() {

	font.loadFont("frabk.ttf", 12);
	check_rc(context.Init(),"initialise context");

	{
		XnLicense license;
		strcpy(license.strVendor,"PrimeSense");
		strcpy(license.strKey,"0KOIk2JeIBYClPWVnMoRKn5cdY4=");
		check_rc(context.AddLicense(license),"add license");
	}
	
	check_rc(depthGenerator.Create(context),"create depth generator");
	check_rc(depthGenerator.SetMapOutputMode(fps_map_mode(30)),"config depth generator");
	check_rc(sceneAnalyzer.Create(context),"create scene analyzer");
	check_rc(sceneAnalyzer.SetMapOutputMode(fps_map_mode(30)),"config scene analyzer");
	check_rc(userGenerator.Create(context),"create user generator");

	//XnCallbackHandle user_cb_handle;
	//userGenerator.RegisterUserCallbacks(NewUserDetected,UserLostDetected,this,user_cb_handle);
	
	max_depth = depthGenerator.GetDeviceMaxDepth();
	printf("Depth generator max depth=%d.\n",max_depth);
	
    imagePixels = new unsigned char[IMGWIDTH*IMGHEIGHT];
	userImage.allocate(IMGWIDTH, IMGHEIGHT, OF_IMAGE_GRAYSCALE);
	for(int i=0;i<MAX_USERS;i++) users[i].present=false;
	ofBackground(255, 255, 255);

	printf("Ready to start generating.\n");
	check_rc( context.StartGeneratingAll(), "start generating");
}


//--------------------------------------------------------------
void testApp::update() {
	if (check_nonfatal(context.WaitOneUpdateAll(depthGenerator),"update")) return;
	
	timestamp = depthGenerator.GetTimestamp();
	got_floor = (sceneAnalyzer.GetFloor(floor_pie)==XN_STATUS_OK);
	if (got_floor) { // project camera onto floor
		XnVector3D origin = {0,0,0};
		project_into_plane(floor_pie, origin, &floor_offset);
	}
	
	const XnDepthPixel *depthMap = depthGenerator.GetDepthMap();
	
	// maybe transfer depth map to image
	if (draw_image==2)
		for (int i=0; i<IMGWIDTH*IMGHEIGHT; i++)
			imagePixels[i] = (255*depthMap[i])/max_depth;
		
	// get pixels for all users, for each compute centroid and bounding rectangle
	{	
		UserStats stats[MAX_USERS];
		
		userGenerator.GetUserPixels(0, sceneMetaData); // 0 for all users
		unsigned short *userRawPixels = (unsigned short*)sceneMetaData.Data();
		
		if (draw_image==1) {
			for (int i=0, y=0; y<IMGHEIGHT; y++) {
				for (int x=0; x<IMGWIDTH; x++, i++) {
					int id=userRawPixels[i];
					if (id>0) {
						stats[id-1].accum(x,y,depthMap[i]);
						imagePixels[i]=(128/MAX_USERS)*id;
					} else imagePixels[i] = 255;
				}
			}
		} else {
			for (int i=0, y=0; y<IMGHEIGHT; y++) {
				for (int x=0; x<IMGWIDTH; x++, i++) {
					int id=userRawPixels[i];
					if (id>0) stats[id-1].accum(x,y,depthMap[i]);
				}
			}
		}

		for (int id0=0; id0<MAX_USERS; id0++) {
			if (stats[id0].area()>0) {
				if (!users[id0].present) {
					send_osc_i(sender,"/birth",id0+1);
					users[id0].present=true;
				}
				update_user(id0+1,stats[id0],&users[id0]);
			} else {
				if (users[id0].present) {
					send_osc_i(sender,"/death",id0+1);
					users[id0].present=false;
				}
			}
		}
				
	}
	if (draw_image) {
		userImage.setFromPixels(imagePixels, IMGWIDTH, IMGHEIGHT, OF_IMAGE_GRAYSCALE);
	}
}

void testApp::update_user(int id, UserStats& stats, UserData *user) {
	XnVector3D    proj[3], real[3];
	int           *coors=user->plot_coors;
	
	stats.centroid(&proj[0]);	// centroid in device coordinate
	stats.left.cpy(&proj[1]);	// left edge in device coordinates
	stats.right.cpy(&proj[2]);	// right edge in device coordinates
	
	// save values required for drawing later
	coors[0]=floor(proj[0].X); 
	coors[1]=floor(proj[0].Y);
	coors[2]=floor(proj[0].Z);
	coors[3]=stats.left.X;		
	coors[4]=stats.right.X;
	coors[5]=stats.top.Y;		
	coors[6]=stats.bottom.Y;
	
	if (got_floor) {
		ofxOscMessage m;
		XnVector3D    on_floor;
		
		// convert to camera-centric world coordinates
		depthGenerator.ConvertProjectiveToRealWorld(3,proj,real);
		
		// projection of centroid onto floor...
		project_into_plane(floor_pie,real[0],&on_floor);
		
		// ...relative to projection of camera on floor
		on_floor.X -= floor_offset.X;
		on_floor.Y -= floor_offset.Y;
		on_floor.Z -= floor_offset.Z;
		
		m.setAddress("/track");
		m.addIntArg(id);
		
		m.addFloatArg(on_floor.X);
		// m.addFloatArg(on_floor.Y); // not interesting
		m.addFloatArg(on_floor.Z);
		
		{	// distances to edges of bounding box
			// corrected for perspective
			float z=coors[2]; // depth
			m.addIntArg(z*(coors[0]-coors[3]));
			m.addIntArg(z*(coors[4]-coors[0]));
			m.addIntArg(z*(coors[1]-coors[5]));
			m.addIntArg(z*(coors[6]-coors[1]));
			// area, corrected for perspective
			m.addIntArg(z*sqrt(stats.area()));
		}
		
		sender.sendMessage(m);
	}
}

//--------------------------------------------------------------
static inline void draw_rect(float x1, float x2, float y1, float y2){
	ofLine(x1,y1,x1,y2);
	ofLine(x1,y1,x2,y1);
	ofLine(x1,y2,x2,y2);
	ofLine(x2,y1,x2,y2);
}


void testApp::draw(){
	char buf[64];
	ofSetColor(255,255,255);
	
    if (draw_image) userImage.draw(0, 0, IMGWIDTH, IMGHEIGHT);
	// ofSetColor(0);
	// ofLine(IMGWIDTH,0,IMGWIDTH,IMGHEIGHT);

	if (draw_time) {
		ofSetColor(180,0,0);
		sprintf(buf,"%llu",timestamp);
		font.drawString(buf,0,IMGHEIGHT-8);
	}
	
	for(int k=0; k<MAX_USERS; k++) {
		if (users[k].present) {
			int *coors=users[k].plot_coors; // pointer to coors for this user
			int *color=Colors[k%6];
			
			ofSetColor(color[0],color[1],color[2]);
			ofRect(coors[0],coors[1],10,10);
			font.drawString(ofToString(k+1), coors[0]-10, coors[1]-10);
			draw_rect(coors[3],coors[4],coors[5],coors[6]);
			// ofRect(coors[0]+IMGWIDTH, (IMGHEIGHT*coors[2])/max_depth, 10, 10);
		}
	}
	if (!got_floor) { //ofSetColor(0,200,0);
		ofSetColor(255,0,0);
		ofRect(16,16,16,16);
	}
}

void testApp::keyPressed(int key){
	switch (key) {
		case 'd': draw_image = (draw_image+1)%3; break;
		case 'f':
			printf("floor plane: (%f,%f,%f), <%f,%f,%f>\n",
				floor_pie.ptPoint.X, floor_pie.ptPoint.Y, floor_pie.ptPoint.Z,
				floor_pie.vNormal.X, floor_pie.vNormal.Y, floor_pie.vNormal.Z);
			break;
		case 't': draw_time = !draw_time; break;
	}
}

void testApp::keyReleased(int key) {}
void testApp::mouseMoved(int x, int y ) {}
void testApp::mouseDragged(int x, int y, int button) {}
void testApp::mousePressed(int x, int y, int button) {}
void testApp::mouseReleased(int x, int y, int button) {}
void testApp::windowResized(int w, int h) {}