view termapp/termapp.cpp @ 11:a8995f4a5793

Added command line version of tracker, need OpenNI but not OpenFrameworks.

#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;
}