/**
* @brief WriteEXR writes an .exr file.
* @param nameFile
* @param data
* @param width
* @param height
* @param channels
* @param pixelBuffer
* @return
*/
PIC_INLINE bool WriteEXR(std::string nameFile, const float *data, int width,
int height, int channels = 3, Imf::Rgba *pixelBuffer = NULL)
{
Imath::Box2i win;
win.max.x = width - 1;
win.max.y = height - 1;
win.min.x = 0;
win.min.y = 0;
Imf::RgbaOutputFile outC(nameFile.c_str(), win, win, Imf::WRITE_RGBA);
//Copy data
int tot = width * height;
bool bPixelBufferNULL = false;
if(pixelBuffer == NULL) {
pixelBuffer = new Imf::Rgba[tot];
bPixelBufferNULL = true;
}
int j = 0;
for(int i = 0; i < tot; i++) {
pixelBuffer[i].r = data[j];
j++;
pixelBuffer[i].g = data[j];
j++;
pixelBuffer[i].b = data[j];
j++;
pixelBuffer[i].a = 1.0f;
}
outC.setFrameBuffer(pixelBuffer, 1, width);
outC.writePixels(height);
if(bPixelBufferNULL) {
delete[] pixelBuffer;
}
return true;
}
int run(struct bf_machine *machine)
{
long mem_size = MEM_SIZE;
for(;machine->times > 0; machine->times--){
switch(*machine->b & 0x70){ // 01110000b
case 0x00: // ,
#ifdef VERBOSE
printf(",");
#endif
inC(machine->mem_p);
break;
case 0x10: // +
#ifdef VERBOSE
printf("+");
#endif
if(machine->times > 1){
*machine->mem_p += machine->times;
machine->times = 0;
}
else
*machine->mem_p++;
break;
case 0x20: // >
#ifdef VERBOSE
printf(">");
#endif
if(*machine->mem_p >= mem_size){
printf("Fatal programer stupid!\nStop! traing to acess mem after end of BF machine\n");
exit(1);
}
machine->mem_p++;
break;
case 0x30: // [
#ifdef VERBOSE
printf("[");
#endif
if(*machine->mem_p){
machine->stack_p++;
*machine->stack_p = machine->b;
}
else
while(*machine->b != ']') machine->b++;
break;
case 0x40: // .
#ifdef VERBOSE
printf(".");
#endif
outC(machine->mem_p);
break;
case 0x50: // -
#ifdef VERBOSE
printf("-");
#endif
if(machine->times > 1){
*machine->mem_p -= machine->times;
machine->times = 0;
}
else
*machine->mem_p--;
break;
case 0x60: // <
#ifdef VERBOSE
printf("<");
#endif
if(machine->mem_p < machine->mem){
printf("Fatal programer stupid!\nStop! traing to acess mem before star of BF machine\n");
exit(1);
}
machine->mem_p--;
break;
case 0x70: // ]
#ifdef VERBOSE
printf("]");
#endif
machine->b = *machine->stack_p--;
break;
}
}
}
int main(int argc, char **argv)
{
int res;
try {
socket.bind ("tcp://*:14444");
s_sendmore (socket, "event");
s_send (socket, "{type:\"up\"}");
}
catch (zmq::error_t e) {
cerr << "Cannot bind to socket: " <<e.what() << endl;
return -1;
}
// printf("Kinect camera test\n");
//
// int i;
// for (i=0; i<2048; i++) {
// float v = i/2048.0;
// v = powf(v, 3)* 6;
// t_gamma[i] = v*6*256;
// }
//
// g_argc = argc;
// g_argv = argv;
//
// //setup Freenect...
// if (freenect_init(&f_ctx, NULL) < 0) {
// printf("freenect_init() failed\n");
// return 1;
// }
//
// freenect_set_log_level(f_ctx, FREENECT_LOG_ERROR);
//
// int nr_devices = freenect_num_devices (f_ctx);
// printf ("Number of devices found: %d\n", nr_devices);
//
// int user_device_number = 0;
// if (argc > 1)
// user_device_number = atoi(argv[1]);
//
// if (nr_devices < 1)
// return 1;
//
// if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
// printf("Could not open device\n");
// return 1;
// }
//
// freenect_set_tilt_degs(f_dev,freenect_angle);
// freenect_set_led(f_dev,LED_RED);
// freenect_set_depth_callback(f_dev, depth_cb);
// freenect_set_video_callback(f_dev, rgb_cb);
// freenect_set_video_format(f_dev, FREENECT_VIDEO_RGB);
// freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);
//
// freenect_start_depth(f_dev);
// freenect_start_video(f_dev);
initFreenect();
//start the freenect thread to poll for events
res = pthread_create(&ocv_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
Mat depthf;
Mat frameMat(rgbMat);
Mat blobMaskOutput(frameMat.size(),CV_8UC1),
outC(frameMat.size(),CV_8UC3);
Mat prevImg(frameMat.size(),CV_8UC1),
nextImg(frameMat.size(),CV_8UC1),
prevDepth(depthMat.size(),CV_8UC1);
vector<Point2f> prevPts,nextPts;
vector<uchar> statusv;
vector<float> errv;
Rect cursor(frameMat.cols/2,frameMat.rows/2,10,10);
bool update_bg_model = true;
int fr = 1;
int register_ctr = 0,register_secondbloc_ctr = 0;
bool registered = false;
Point2i appear(-1,-1); double appearTS = -1;
Point2i midBlob(-1,-1);
Point2i lastMove(-1,-1);
int hcr_ctr = -1;
vector<int> hc_stack(20); int hc_stack_ptr = 0;
while (!die) {
fr++;
// imshow("rgb", rgbMat);
pthread_mutex_lock(&buf_mutex);
//Linear interpolation
{
Mat _tmp = (depthMat - 400.0); //minimum observed value is ~440. so shift a bit
_tmp.setTo(Scalar(2048), depthMat > ((!registered) ? 700.0 : 750.0)); //cut off at 600 to create a "box" where the user interacts
_tmp.convertTo(depthf, CV_8UC1, 255.0/1648.0); //values are 0-2048 (11bit), account for -400 = 1648
}
{
Mat _tmp;
depthMat.convertTo(_tmp, CV_8UC1, 255.0/2048.0);
cvtColor(_tmp, outC, CV_GRAY2BGR);
}
pthread_mutex_unlock(&buf_mutex);
// { //saving the frames to files for debug
// stringstream ss; ss << "depth_"<<fr<<".png";
// imwrite(ss.str(), depthf);
// }
//Logarithm interpolation - try it!, It should be more "sensitive" for closer depths
// {
// Mat tmp,tmp1;
// depthMat.convertTo(tmp, CV_32FC1);
// log(tmp,tmp1);
// tmp1.convertTo(depthf, CV_8UC1, 255.0/7.6246189861593985);
// }
// imshow("depth",depthf);
Mat blobMaskInput = depthf < 255; //anything not white is "real" depth
vector<Point> ctr,ctr2;
Scalar blb = refineSegments(Mat(),blobMaskInput,blobMaskOutput,ctr,ctr2,midBlob); //find contours in the foreground, choose biggest
imshow("first", blobMaskOutput);
/////// blb :
//blb[0] = x, blb[1] = y, blb[2] = 1st blob size, blb[3] = 2nd blob size.
// uint mode_counters[3] = {0};
if(blb[0]>=0 && blb[2] > 500) { //1st blob detected, and is big enough
//cvtColor(depthf, outC, CV_GRAY2BGR);
//closest point to the camera
Point minLoc; double minval,maxval;
minMaxLoc(depthf, &minval, &maxval, &minLoc, NULL, blobMaskInput);
circle(outC, minLoc, 5, Scalar(0,255,0), 3);
Scalar mn,stdv;
meanStdDev(depthf,mn,stdv,blobMaskInput);
//cout << "min: " << minval << ", max: " << maxval << ", mean: " << mn[0] << endl;
blobMaskInput = depthf < (mn[0] + stdv[0]*.5);
blb = refineSegments(Mat(),blobMaskInput,blobMaskOutput,ctr,ctr2,midBlob);
imshow("second", blobMaskOutput);
if(blb[0] >= 0 && blb[2] > 300) {
//draw contour
Scalar color(0,0,255);
for (int idx=0; idx<ctr.size()-1; idx++)
line(outC, ctr[idx], ctr[idx+1], color, 1);
line(outC, ctr[ctr.size()-1], ctr[0], color, 1);
if(ctr2.size() > 0) {
Scalar color2(255,0,255);
for (int idx=0; idx<ctr2.size()-1; idx++)
line(outC, ctr2[idx], ctr2[idx+1], color2, 2);
line(outC, ctr2[ctr2.size()-1], ctr2[0], color2, 2);
}
//draw "major axis"
// Vec4f _line;
Mat curve(ctr);
// fitLine(curve, _line, CV_DIST_L2, 0, 0.01, 0.01);
// line(outC, Point(blb[0]-_line[0]*70,blb[1]-_line[1]*70),
// Point(blb[0]+_line[0]*70,blb[1]+_line[1]*70),
// Scalar(255,255,0), 1);
//blob center
circle(outC, Point(blb[0],blb[1]), 50, Scalar(255,0,0), 3);
// cout << "min depth " << minval << endl;
register_ctr = MIN((register_ctr + 1),60);
if(blb[3] > 5000)
register_secondbloc_ctr = MIN((register_secondbloc_ctr + 1),60);
if (register_ctr > 30 && !registered) {
registered = true;
appear.x = -1;
update_bg_model = false;
lastMove.x = blb[0]; lastMove.y = blb[1];
cout << "blob size " << blb[2] << endl;
if(register_secondbloc_ctr < 30) {
if(blb[2] > 10000) {
cout << "register panner" << endl;
send_event("Register", "\"mode\":\"openhand\"");
} else {
cout << "register pointer" << endl;
send_event("Register", "\"mode\":\"theforce\"");
}
} else {
cout << "register tab swithcer" << endl;
send_event("Register", "\"mode\":\"twohands\"");
}
}
if(registered) {
stringstream ss;
ss << "\"x\":" << (int)floor(blb[0]*100.0/640.0)
<< ",\"y\":" << (int)floor(blb[1]*100.0/480.0)
<< ",\"z\":" << (int)(mn[0] * 2.0);
//cout << "move: " << ss.str() << endl;
send_event("Move", ss.str());
//---------------------- fist detection ---------------------
//calc laplacian of curve
vector<Point> approxCurve; //approximate curve
approxPolyDP(curve, approxCurve, 10.0, true);
Mat approxCurveM(approxCurve);
Mat curve_lap;
calc_laplacian(approxCurveM, curve_lap); //calc laplacian
hcr_ctr = 0;
for (int i=0; i<approxCurve.size(); i++) {
double n = norm(((Point2d*)(curve_lap.data))[i]);
if (n > 10.0) {
//high curvature point
circle(outC, approxCurve[i], 3, Scalar(50,155,255), 2);
hcr_ctr++;
}
}
hc_stack.at(hc_stack_ptr) = hcr_ctr;
hc_stack_ptr = (hc_stack_ptr + 1) % hc_stack.size();
Scalar _avg = mean(Mat(hc_stack));
if (abs(_avg[0] - (double)hcr_ctr) > 5.0) { //a big change in curvature = hand fisted/opened?
cout << "Hand click!" << endl;
send_event("HandClick", "");
}
if (mode_state == MODE_NONE) {
}
// imshow("out",out);
//doHist(depthf,out);
{ //some debug on screen..
stringstream ss; ss << "high curve pts " << hcr_ctr << ", avg " << _avg[0];
putText(outC, ss.str(), Point(50,50), CV_FONT_HERSHEY_PLAIN, 2.0,Scalar(0,0,255), 2);
}
} else {
//not registered, look for gestures
if(appear.x<0) {
//first appearence of blob
appear = midBlob;
// update_bg_model = false;
appearTS = getTickCount();
cout << "appear ("<<appearTS<<") " << appear.x << "," << appear.y << endl;
} else {
//blob was seen before, how much time passed
double timediff = ((double)getTickCount()-appearTS)/getTickFrequency();
if (timediff > .2 && timediff < 1.0) {
//enough time passed from appearence
line(outC, appear, Point(blb[0],blb[1]), Scalar(0,0,255), 3);
if (appear.x - blb[0] > 100) {
cout << "right"<<endl; appear.x = -1;
send_event("SwipeRight", "");
update_bg_model = true;
register_ctr = 0;
} else if (appear.x - blb[0] < -100) {
cout << "left" <<endl; appear.x = -1;
send_event("SwipeLeft", "");
update_bg_model = true;
register_ctr = 0;
} else if (appear.y - blb[1] > 100) {
cout << "up" << endl; appear.x = -1;
send_event("SwipeUp", "");
update_bg_model = true;
register_ctr = 0;
} else if (appear.y - blb[1] < -100) {
cout << "down" << endl; appear.x = -1;
send_event("SwipeDown", "");
update_bg_model = true;
register_ctr = 0;
}
}
if(timediff >= 1.0) {
cout << "a ghost..."<<endl;
update_bg_model = true;
//a second passed from appearence - reset 1st appear
appear.x = -1;
appearTS = -1;
midBlob.x = midBlob.y = -1;
}
}
}
send_image(outC);
}
} else {
send_image(depthf);
register_ctr = MAX((register_ctr - 1),0);
register_secondbloc_ctr = MAX((register_secondbloc_ctr - 1),0);
}
imshow("blob",outC);
if (register_ctr <= 15 && registered) {
midBlob.x = midBlob.y = -1;
registered = false;
mode_state = MODE_NONE;
update_bg_model = true;
cout << "unregister" << endl;
send_event("Unregister", "");
}
char k = cvWaitKey(5);
if( k == 27 ) break;
if( k == ' ' )
update_bg_model = !update_bg_model;
if (k=='s') {
cout << "send test event" << endl;
send_event("TestEvent", "");
}
}
printf("-- done!\n");
pthread_join(ocv_thread, NULL);
pthread_exit(NULL);
return 0;
}
int main(int argc, char *argv[])
{
MACRO_VARS // :.
int times,f_name_i = 1;
byte b,nible,*loaded_file,stack_i;
struct space *f;
uint stack[256];
/* BF VM vars */
byte *mem;
uint pos = 0;
/* end */
if(argc == 3){
f_name_i = 2;
if(argv[1][0] == '-' && argv[1][1] == 'v')
v = 1;
}
f = loadfile(argv[f_name_i]);
loaded_file = f->p;
mem = alloc_mem(MEM_SIZE);
nible = 0; // starting by nible 0
times = 1; // run a command, at least once
b = GETB();
if(v) printf("b = 0x%x\n", b);
while(b){
if(b & BIT7){
if(v) printf("Isn't a char\n");
if(nible){
if(v) printf("Second nible\n");
if(b & BIT3){ // it is XXXX1XXXb?
if(v) printf("Is a number of times to repeat\n");
switch(b & 0x07){ // 00000111b
case 0: times = 0; break;
case 1: times = 2; break;
case 2: times = 4; break;
case 3: times = 8; break;
case 4: times = 16; break;
case 5: times = 32; break;
case 6: times = 64; break;
case 7: times = 128; break;
}
if(times == 0){
}
if(v) printf("Repeat %i\n", times);
}
else{
if(v) printf("Is a command\n");
times = 1;
b <<= 4;
b |= 0x80; // set first bt
}
}
else{
if(v) printf("First nible\n");
times = 1;
}
for(; times >= 1; times--){
if(v) printf("Executando a %i_a vez\n", times);
switch(b & 0x70){ // 01110000b
case 0x00: // ,
//if(v) printf("Digite um caracter:\n");
mem[pos] = inC();
break;
case 0x10: // +
if(times > 1){
mem[pos] += times;
if(v) printf("%i x +\n", times);
times = 0;
}
else{
if(v) printf("+\n");
mem[pos]++;
}
break;
case 0x20: // >
if(v) printf(">\n");
if(pos >= MEM_SIZE){
printf("Fatal programer stupid!\nStop! traing to acess mem after end of BF machine\n");
exit(1);
}
pos++;
break;
case 0x30: // [
if(v) printf("[\n");
if(mem[pos])
stack[stack_i++] = MACRO_i;
else
while(b != ']') b = GETB();
break;
case 0x40: // .
outC(mem[pos]);
if(v) printf(" = 0x%x\n", mem[pos]);
break;
case 0x50: // -
if(times > 1){
mem[pos] -= times;
if(v) printf("%i x -\n", times);
times = 0;
}
else{
if(v) printf("-\n");
mem[pos]--;
}
break;
case 0x60: // <
if(v) printf("<\n");
if(pos <= 0){
printf("Fatal programer stupid!\nStop! traing to acess mem before star of BF machine\n");
exit(1);
}
pos--;
break;
case 0x70: // ]
if(v) printf("]\n");
MACRO_i = stack[stack_i--];
break;
}
}
if(!nible) nible = 1;
else{
b = GETB();
if(v) printf("b = 0x%x\n", b);
nible = 0;
}
}
else{
if(v) printf("%c", b); //printf("Is a char: %c\n", b);
nible = 0;
b = GETB();
}
}
//printf("%s", mem);
printf("\n");
return(0);
}
