From fa43303ae73e1db3a33436cc1fc5e2fa7dcb68c5 Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@atd.theater.digital>
Date: Wed, 11 Sep 2019 16:31:53 +0200
Subject: [PATCH] working kinect to box implementation

---
 kinectv1_wuerfel.pde | 408 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 408 insertions(+)
 create mode 100644 kinectv1_wuerfel.pde

diff --git a/kinectv1_wuerfel.pde b/kinectv1_wuerfel.pde
new file mode 100644
index 0000000..4c33edf
--- /dev/null
+++ b/kinectv1_wuerfel.pde
@@ -0,0 +1,408 @@
+import org.openkinect.freenect.*;
+import org.openkinect.processing.*;
+import processing.serial.*;
+
+//zadig 2.0.1 libusbK  https://zadig.akeo.ie/downloads/
+
+Serial sPort;
+
+Kinect kinect;
+
+// Depth image
+PImage depthImg;
+PImage depthImgRaw;
+PImage videoImg;
+
+public enum Mode
+{
+  DEPTHCOLOR, NEARESTIMAGECOLOR
+}
+
+Mode mode=Mode.DEPTHCOLOR;
+
+boolean leftview_depth=true;
+boolean enable_mirrorImage=true;
+boolean enable_IRImage=false;
+boolean enable_colordepth=false;
+
+// Which pixels do we care about?
+int minDepth =  200;
+int maxDepth = 1000;
+
+// What is the kinect's angle
+float angle;
+
+static int boxnum=19;
+color box[] = new color[boxnum];
+int boxid[] = new int[boxnum];
+PVector boxpos[] = new PVector[boxnum]; //center position of box
+PVector boxsize[] = new PVector[boxnum]; //size of box (width,height)
+boolean boxenabled[] = new boolean[boxnum];
+float[] depthLookUp = new float[2048];
+
+static int FRAMERATE=10;
+int fadetime=1000/FRAMERATE /10; //fadetime in centiseconds
+
+int editbox=0; //num of box
+boolean editboxpos=false; //editmode
+
+int infomessagecounter=0; //counts every frame
+String infomessage="";
+
+
+void setup() {
+  size(1280, 480);
+  frameRate(FRAMERATE);
+  
+  colorMode(HSB, 255);
+  
+  kinect = new Kinect(this);
+  kinect.initDepth();
+  kinect.initVideo();
+  
+  kinect.enableMirror(enable_mirrorImage);
+  kinect.enableIR(enable_IRImage);
+  kinect.enableColorDepth(enable_colordepth);
+  angle = kinect.getTilt();
+  
+
+  // Blank image
+  depthImgRaw = new PImage(kinect.width, kinect.height);
+  depthImg = new PImage(kinect.width, kinect.height);
+  
+  for (int i=0;i<boxnum;i++) { //initialize boxpositions PVector
+    boxpos[i]=new PVector();
+    boxenabled[i]=false;
+  }
+  
+  //default config
+  for (int i=0;i<boxnum;i++) { //initialize boxsizes
+    boxsize[i]=new PVector();
+    //boxsize[i].x=kinect.width/(boxnum+1);
+    //boxsize[i].y=kinect.width/(boxnum+1);
+    boxsize[i].x=kinect.width/(10+1);
+    boxsize[i].y=kinect.width/(10+1);
+  }
+  
+  int _boxesperrow=10;
+  for (int j=0;j<=int(boxnum/_boxesperrow);j++) {
+    int _boxesthisrow=min(_boxesperrow,boxnum-j*_boxesperrow);
+    for (int i=0;i<_boxesthisrow;i++) { //initialize boxpositions
+      boxpos[i+j*10].x=(i+1)*(kinect.width/(_boxesthisrow+1)); //divide screen equally
+      boxpos[i+j*10].y=kinect.height/2-boxsize[i+j*10].y*j;
+      boxenabled[i+j*10]=true;
+    }
+  }
+  
+  boxid[0]=15;
+  boxid[1]=11;
+  boxid[2]=3;
+  boxid[3]=18;
+  boxid[4]=2;
+  boxid[5]=30;
+  boxid[6]=7;
+  boxid[7]=1;
+  boxid[8]=27;
+  boxid[9]=23;
+  boxid[10]=6;
+  boxid[11]=22;
+  boxid[12]=14;
+  boxid[13]=26;
+  boxid[14]=4;
+  boxid[15]=16;
+  boxid[16]=28;
+  boxid[17]=12;
+  boxid[18]=21;
+  
+  String portName = Serial.list()[0];
+  sPort = new Serial(this, portName, 115200);
+  
+  for (int i = 0; i < depthLookUp.length; i++) {
+    depthLookUp[i] = rawDepthToMeters(i);
+  }
+  
+  
+  loadBoxConfig();
+}
+
+float rawDepthToMeters(int depthValue) {
+  if (depthValue < 2047) {
+    return (float)(1.0 / ((double)(depthValue) * -0.0030711016 + 3.3309495161));
+  }
+  return 0.0f;
+}
+
+
+void draw() {
+  videoImg=kinect.getVideoImage();
+  // Draw the raw image
+  if (leftview_depth) {
+    image(kinect.getDepthImage(), 0, 0);
+  } else {
+    image(videoImg, 0, 0);
+  }
+  
+  
+  // Threshold the depth image
+  int[] rawDepth = kinect.getRawDepth();
+  for (int i=0; i < rawDepth.length; i++) {
+    float depthMeters=rawDepthToMeters(rawDepth[i]);
+    if (rawDepthToMeters(rawDepth[i])<=0.0) { //depth can not be determined
+      depthMeters=rawDepthToMeters(maxDepth); //set to maximum
+    }
+    depthImg.pixels[i] = color(constrain(map(depthMeters,rawDepthToMeters(minDepth),rawDepthToMeters(maxDepth),255,0) ,0,255)) ; //depth by brightness
+  }
+  
+  
+
+  // Draw the thresholded image
+  depthImg.updatePixels();
+  image(depthImg, kinect.width, 0);
+  
+  
+  
+
+  for (int i=0;i<boxnum;i++) {
+    float cpmean=0;
+    int cpmeancounter=0;
+    float cpmax=0;
+    
+    float videocolor_nearestR=0;
+    float videocolor_nearestG=0;
+    float videocolor_nearestB=0;
+    float videocolor_nearest_counter=0;
+    
+    PVector _boxsize=boxsize[i];
+    for (int offy=int(-_boxsize.y/2);offy<_boxsize.y/2;offy++) {
+      for (int offx=int(-_boxsize.x/2);offx<_boxsize.x/2;offx++) {
+        PVector _pos=new PVector(boxpos[i].x,boxpos[i].y);
+        _pos.x+=offx;
+        _pos.y+=offy;
+      
+        color cp = depthImg.get(int(_pos.x),int(_pos.y));
+        if (cpmax<=brightness(cp)){
+          cpmax=brightness(cp);
+        }
+        cpmean+=brightness(cp); //the brighter the closer
+        cpmeancounter++;
+        
+        //video image
+        color _vidcolor=videoImg.get(int(_pos.x),int(_pos.y));
+        videocolor_nearestR+=red(_vidcolor)*(brightness(cp)/255.0);
+        videocolor_nearestG+=green(_vidcolor)*(brightness(cp)/255.0);
+        videocolor_nearestB+=blue(_vidcolor)*(brightness(cp)/255.0);
+        videocolor_nearest_counter++;
+      }
+    }
+    cpmean/=cpmeancounter;
+    videocolor_nearestR/=videocolor_nearest_counter;
+    videocolor_nearestG/=videocolor_nearest_counter;
+    videocolor_nearestB/=videocolor_nearest_counter;
+    colorMode(RGB, 255);
+    color videocolor_nearest=color(videocolor_nearestR,videocolor_nearestG,videocolor_nearestB);
+    colorMode(HSB, 255);
+    
+    
+    //draw box
+    noFill();
+    stroke(150,255,100);
+    if (!boxenabled[i]){
+      stroke(0,255,100); //different rect color if disabled
+    }
+    rectMode(CENTER);
+    textSize(12);
+    textAlign(LEFT,TOP);
+    rect(kinect.width+boxpos[i].x,boxpos[i].y, boxsize[i].x,boxsize[i].y); //draw on right image
+    if (editboxpos && editbox==i) { //highlight current edited box
+      line(kinect.width+boxpos[i].x-boxsize[i].x/2,boxpos[i].y-boxsize[i].y/2,kinect.width+boxpos[i].x+boxsize[i].x/2,boxpos[i].y+boxsize[i].y/2);
+      line(kinect.width+boxpos[i].x+boxsize[i].x/2,boxpos[i].y-boxsize[i].y/2,kinect.width+boxpos[i].x-boxsize[i].x/2,boxpos[i].y+boxsize[i].y/2);
+    }
+    
+    fill(box[i]); //preview boxcolor
+    noStroke();
+    rectMode(CENTER);
+    rect(kinect.width+boxpos[i].x,boxpos[i].y, boxsize[i].x/8,boxsize[i].y/8); //draw on right image
+    
+    fill(150,255,100); //textcolor
+    text(""+i+"|"+boxid[i],kinect.width+boxpos[i].x-boxsize[i].x/2,boxpos[i].y-boxsize[i].y/2);
+    println("boxpos["+i+"].x="+boxpos[i].x+",boxpos["+i+"].y="+boxpos[i].y+", boxsize["+i+"].x="+boxsize[i].x+",boxsize["+i+"].y="+boxsize[i].y);
+    
+    
+    int brightness=0;
+    if (cpmax>0){
+      brightness=255;
+    }
+    if (mode.equals(Mode.DEPTHCOLOR)){ //depth color
+      box[i]=color(map(cpmax,0,255, 0,255),255,brightness);
+    }else if(mode.equals(Mode.NEARESTIMAGECOLOR)) { //image color
+      box[i]=color(hue(videocolor_nearest),saturation(videocolor_nearest),brightness);
+    }
+    if (i==5){
+      println("cpmean="+cpmean);
+      println("cpmax="+cpmax);
+    }
+    
+  }
+
+  sendToWuerfels();
+
+  textSize(12);
+  fill(0,255,100); //textcolor
+  text("TILT: " + angle, 10, 20);
+  text("THRESHOLD: [" + minDepth + ", " + maxDepth + "]", 10, 20+1*16);
+  if(editboxpos){
+    text("Edit Boxpos box="+editbox, 10, 20+2*16);
+  }
+  
+  if (infomessagecounter>0){
+    fill(150,200,255); //textcolor
+    text(infomessage,10,height-20);
+    infomessagecounter--;
+  }
+}
+
+// Adjust the angle and the depth threshold min and max
+void keyPressed() {
+  if (key == CODED && !editboxpos) {
+    if (keyCode == UP) {
+      angle++;
+    } else if (keyCode == DOWN) {
+      angle--;
+    }
+    angle = constrain(angle, 0, 30);
+    kinect.setTilt(angle);
+  } else if (key == 'x') {
+    minDepth = constrain(minDepth+10, 0, maxDepth);
+  } else if (key == 'y') {
+    minDepth = constrain(minDepth-10, 0, maxDepth);
+  } else if (key == 'v') {
+    maxDepth = constrain(maxDepth+10, minDepth, 2047);
+  } else if (key =='c') {
+    maxDepth = constrain(maxDepth-10, minDepth, 2047);
+  } else if (key =='e') { //toggle edit boxpos mode
+    editboxpos=!editboxpos;
+    infomessagecounter=FRAMERATE*1;
+    infomessage="editboxpos="+editboxpos;
+  } else if (key =='s') { //save boxes
+    saveBoxConfig();
+  } else if (key =='l') { //load boxes
+    loadBoxConfig();
+  } else if (key =='u') { //switch left view
+    leftview_depth=!leftview_depth;
+  } else if (key =='m') { // mirror
+    enable_mirrorImage=!enable_mirrorImage;
+    kinect.enableMirror(enable_mirrorImage);
+  } else if (key =='i') { // toggle IR
+    enable_IRImage=!enable_IRImage;
+    kinect.enableIR(enable_IRImage);
+  } else if (key =='k') { // toggle IR
+    enable_colordepth=!enable_colordepth;
+    kinect.enableColorDepth(enable_colordepth);
+  } else if (key =='w') { // switch mode
+    if (mode.equals(Mode.DEPTHCOLOR)){
+      mode=Mode.NEARESTIMAGECOLOR;
+    }else if (mode.equals(Mode.NEARESTIMAGECOLOR)){
+      mode=Mode.DEPTHCOLOR;
+    }
+    infomessagecounter=FRAMERATE*2;
+    infomessage="Mode="+mode.toString();
+  } 
+  
+  
+  
+  
+  if (editboxpos){ //in boxposedit mode
+    if (key == '-') { //change boxedit id
+      if (editbox>0){
+        editbox--;
+      }
+    } else if (key == '+') { //change boxedit id
+      if (editbox<boxnum-1){
+        editbox++;
+      }
+    } else if (key == '4') { //change boxsize 
+      boxsize[editbox].x--;
+    } else if (key == '6') { //change boxsize 
+      boxsize[editbox].x++;
+    } else if (key == '2') { //change boxsize 
+      boxsize[editbox].y--;
+    } else if (key == '8') { //change boxsize 
+      boxsize[editbox].y++;
+    } else if (key == '*') { //apply this size to all boxes
+      for (int ii=0;ii<boxnum;ii++) {
+        if (ii!=editbox) {
+          boxsize[ii].x=boxsize[editbox].x;
+          boxsize[ii].y=boxsize[editbox].y;
+        }
+      }
+    }
+    if (key == CODED) {
+      if (keyCode == LEFT) {
+        boxpos[editbox].x-=1;
+      } else if (keyCode == RIGHT) {
+        boxpos[editbox].x+=1;
+      }else if (keyCode == UP) {
+        boxpos[editbox].y-=1;
+      } else if (keyCode == DOWN) {
+        boxpos[editbox].y+=1;
+      }
+    }
+    
+    infomessagecounter=FRAMERATE*2;
+    infomessage=boxpos[editbox].x+", "+boxpos[editbox].y+": "+boxsize[editbox].x+", "+boxsize[editbox].y;
+  }
+}
+
+void sendToWuerfels() {
+  for (int i=0;i<boxnum;i++) {
+    if (boxenabled[i]) {
+      //print(str(i)+"="+str(brightness(box[i]))+", ");
+      sPort.write("B,"+str(boxid[i])+","+str(fadetime)+","+int(red(box[i]))+","+int(green(box[i]))+","+int(blue(box[i]))+"\n");
+      print("B,"+str(boxid[i])+","+str(fadetime)+","+int(red(box[i]))+","+int(green(box[i]))+","+int(blue(box[i]))+"\n");
+    }
+  }
+  println();
+}
+
+void mouseClicked() {
+  if (editboxpos) {
+    int _newposx=(mouseX-kinect.width);
+    int _newposy=mouseY;
+    boxpos[editbox].x=_newposx;
+    boxpos[editbox].y=_newposy;
+    println("Click "+_newposx+", "+_newposy);
+    infomessagecounter=FRAMERATE*2;
+    infomessage="x="+_newposx+" y="+_newposy;
+  }
+}
+
+void saveBoxConfig(){
+  String[] boxconfig=new String[boxnum];
+  for (int i=0;i<boxnum;i++) {
+    boxconfig[i]=boxid[i]+";"+int(boxpos[i].x)+";"+int(boxpos[i].y)+";"+int(boxsize[i].x)+";"+int(boxsize[i].y);
+  }
+  saveStrings("boxconfig.txt", boxconfig);
+  println("saved");
+  infomessagecounter=FRAMERATE*2;
+  infomessage="saved";
+}
+
+void loadBoxConfig(){
+  String[] boxconfig = loadStrings("boxconfig.txt");
+  for (int i=0;i<boxnum;i++) { //disable boxes
+    boxenabled[i]=false;
+  }
+  for (int i=0;i<min(boxconfig.length,boxnum);i++) {
+    String[] _boxconfsplit=split(boxconfig[i], ';');
+    boxid[i]=int(_boxconfsplit[0]);
+    boxpos[i].x=int(_boxconfsplit[1]);
+    boxpos[i].y=int(_boxconfsplit[2]);
+    boxsize[i].x=int(_boxconfsplit[3]);
+    boxsize[i].y=int(_boxconfsplit[4]);
+    boxenabled[i]=true;
+  }
+  println("loaded");
+  infomessagecounter=FRAMERATE*2;
+  infomessage="loaded";
+  
+}