Mercurial > hg > grouptrack

changeset 11:a8995f4a5793

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Added command line version of tracker, need OpenNI but not OpenFrameworks.
author samer
date Fri, 17 Feb 2012 23:08:53 +0000
parents 3d20dedfd1da
children af71ea70b7f4
files termapp/Makefile termapp/README termapp/termapp termapp/termapp.cpp termtrack.sh
diffstat 5 files changed, 370 insertions(+), 0 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/termapp/Makefile	Fri Feb 17 23:08:53 2012 +0000
@@ -0,0 +1,17 @@
+OPEN_NI_ROOT=/opt/OpenNI
+INCLUDES=-I$(OPEN_NI_ROOT)/usr/include/ni -I/opt/local/include
+LIBS=-L$(OPEN_NI_ROOT)/usr/lib -lOpenNI -L/opt/local/lib -llo -lcurses
+TARGET=termapp
+
+build: $(TARGET)
+
+clean:
+	rm $(TARGET)
+	rm $(TARGET).o
+
+$(TARGET).o: $(TARGET).cpp
+	g++ $(CFLAGS) -c $(INCLUDES) $(TARGET).cpp
+
+$(TARGET): $(TARGET).o
+	g++ $(CFLAGS) -o $(TARGET) $(LIBS) $(TARGET).o
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/termapp/README	Fri Feb 17 23:08:53 2012 +0000
@@ -0,0 +1,17 @@
+This is a command line version of the group tracker.
+It doesn't use OpenFrameworks.
+You still need OpenNI properly installed:
+	1. OpenNI binaries from OpenNI website or build from source on github
+	2. NITE middleware binaries from OpenNI website.
+	3. avin2's SensorKinect module from github
+
+You also need liblo, eg from MacPorts.
+See Makefile for setup.
+
+NB. If you have OpenNI shared libraries installed in a funny place, you
+will need to set DYLD_FALLBACK_LIBRARY_PATH to point to it to run termapp.
+AND you also need OPEN_NI_INSTALL_PATH set too.
+See termtrack.sh in directory above this one for an example - I have my OpenNI
+installed in /opt/OpenNI, so I need
+	OPEN_NI_INSTALL_PATH=/opt/OpenNI
+	DYLD_FALLBACK_LIBRARY_PATH=$OPEN_NI_INSTALL_PATH/usr/lib
Binary file termapp/termapp has changed
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/termapp/termapp.cpp	Fri Feb 17 23:08:53 2012 +0000
@@ -0,0 +1,333 @@
+#include <XnCppWrapper.h>
+#include <lo/lo.h>
+#include <math.h>
+#include <curses.h>
+#include <stdlib.h>
+#include <signal.h>
+
+#define IMGWIDTH  640
+#define IMGHEIGHT 480
+#define MAX_USERS 16
+#define HOST "localhost"
+#define PORT "7770"
+#define EXIT_APP(rc) exit(rc)
+
+using namespace xn;
+
+class UserStats;
+class termapp {
+private:
+	Context        context;
+	DepthGenerator depthGenerator;
+	UserGenerator  userGenerator;
+	SceneAnalyzer  sceneAnalyzer;
+	SceneMetaData  sceneMetaData;
+	XnDepthPixel   max_depth;
+	XnUInt64	   timestamp;
+	XnPlane3D      floor_pie;
+	lo_address     target;
+
+	bool  got_floor;
+	XnVector3D floor_offset;
+	
+	class UserData {
+	public:
+		bool present;
+		int  plot_coors[7];
+	};
+	
+	UserData users[MAX_USERS];
+	
+	void update_user(int id, UserStats& stats, UserData *user);
+	void send_osc_i(const char *path, int arg);
+	void send_track(int id, float x, float y, int *coors, float area);
+	
+public:
+	termapp(const char *host, const char *port, int fps);
+	~termapp() { 
+		lo_address_free(target);
+		printf("Shutting down OpenNI...\n");
+		context.Shutdown();
+	};
+	
+	void setup();
+	void update();
+	void draw();
+	void keyPressed(int);
+};
+
+static inline void check_rc(XnStatus rc, const char *tag){
+	if (rc != XN_STATUS_OK) {
+		printf("FAILURE: %s - %s\n",tag,xnGetStatusString(rc));
+		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++;
+	}
+	
+};
+
+
+//--------------------------------------------------------------
+
+termapp::termapp(const char *host, const char *port, int FPS) {
+
+	// initialise OSC sender
+	printf("OSC target: %s:%s.\n",host,port);
+	// printf("Will run at %d frames per second.\n",FPS);
+	target = lo_address_new(host,port);
+}
+
+static XnMapOutputMode fps_map_mode(int fps) {
+	XnMapOutputMode mapMode;
+	
+	mapMode.nXRes = IMGWIDTH;
+	mapMode.nYRes = IMGHEIGHT;
+	mapMode.nFPS  = fps;
+	return mapMode;
+}
+	
+void termapp::setup() {
+	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);
+	
+	for(int i=0;i<MAX_USERS;i++) users[i].present=false;
+
+	printf("Ready to start generating.\n");
+	check_rc( context.StartGeneratingAll(), "start generating");
+}
+
+
+//--------------------------------------------------------------
+void termapp::update() {
+	if (check_nonfatal(context.WaitOneUpdateAll(depthGenerator),"update")) return;
+	
+	userGenerator.GetUserPixels(0, sceneMetaData); // 0 for all users
+	timestamp = sceneMetaData.Timestamp();
+	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);
+	}
+	
+	// get pixels for all users, for each compute centroid and bounding rectangle
+	{	
+		const XnDepthPixel *depthMap = depthGenerator.GetDepthMap();
+		unsigned short *userRawPixels = (unsigned short*)sceneMetaData.Data();
+		UserStats stats[MAX_USERS];
+		
+		for (int i=0, y=0; y<IMGHEIGHT; y++) {
+			for (int x=0; x<IMGWIDTH; x++, i++) {
+				int id=userRawPixels[i];
+				if (id>0 && id<=MAX_USERS) {
+					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("/birth",id0+1);
+					send_osc_i("/randinit",id0+1);
+					send_osc_i("/start",id0+1);
+					users[id0].present=true;
+				}
+				update_user(id0+1,stats[id0],&users[id0]);
+			} else {
+				if (users[id0].present) {
+					send_osc_i("/death",id0+1);
+					users[id0].present=false;
+				}
+			}
+		}
+	}
+}
+
+void termapp::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) {
+		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;
+		
+		send_track(id, on_floor.X, on_floor.Y, coors, stats.area());
+	}
+}
+
+void termapp::draw(){
+	printf("  % 6.2lf s  floor[%s]  users[",((double)timestamp)/1000000, got_floor ? "*" : ".");
+	for (int id0=0; id0<MAX_USERS; id0++) putchar( users[id0].present ? '*' : '.');
+	printf("]   \r");
+	fflush(stdout);
+}
+
+void termapp::keyPressed(int key){
+	switch (key) {
+		case 'f':
+			printf("\n   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;
+	}
+}
+
+void termapp::send_osc_i(const char *path, int arg) {
+	lo_message msg=lo_message_new();
+	lo_message_add_int32(msg,arg);
+	lo_send_message(target,path,msg);
+	// printf("- sent %s %d.\n",path,arg);
+}
+
+
+void termapp::send_track( int id, float x, float y, int *coors, float area) 
+{
+	lo_message m=lo_message_new();
+	float z=coors[2]; // depth
+
+	lo_message_add_int32(m,id);
+	lo_message_add_float(m,x);
+	lo_message_add_float(m,z);
+
+	// distances to edges of bounding box
+	// corrected for perspective
+	lo_message_add_int32(m,z*(coors[0]-coors[3]));
+	lo_message_add_int32(m,z*(coors[4]-coors[0]));
+	lo_message_add_int32(m,z*(coors[1]-coors[5]));
+	lo_message_add_int32(m,z*(coors[6]-coors[1]));
+
+	// area, corrected for perspective
+	lo_message_add_int32(m,z*sqrt(area));
+
+	lo_send_message(target,"/track",m);
+}
+
+static termapp *app=NULL;
+
+static void signal_handler(int sig) {
+	printf("\nCaught SIGINT - terminating.\n");
+	if (app) { delete app; app=NULL; }
+	exit(0);
+}
+
+//========================================================================
+int main(int argc, const char **argv)
+{
+	int i,ch;
+	app=new termapp(
+	   argc>1 ? argv[1] : HOST, 
+	   argc>2 ? argv[2] : PORT,
+	   argc>3 ? atoi(argv[3]) : 15);
+
+	signal(SIGINT, signal_handler);
+
+	printf("Setting up...\n");
+	app->setup();
+	printf("Running...\n");
+	for (;;) {
+		app->update();
+		app->draw();
+		ch=getch();
+		if (ch!=ERR) {
+			if (ch=='Q') break;
+			printf("\n got key: %d.\n",ch);
+			app->keyPressed(ch);
+		}
+	}
+	delete app;
+	app=NULL;
+}
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/termtrack.sh	Fri Feb 17 23:08:53 2012 +0000
@@ -0,0 +1,3 @@
+#!/bin/sh
+export OPEN_NI_INSTALL_PATH=/opt/OpenNI
+DYLD_FALLBACK_LIBRARY_PATH=$OPEN_NI_INSTALL_PATH/usr/lib termapp/termapp "$@"