diff src/testApp.cpp @ 0:2aa8ba4db20e

Initial check in.

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/testApp.cpp	Sat Jan 21 12:15:29 2012 +0000
@@ -0,0 +1,334 @@
+#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) {}
+