From 7e4c5352b3df802845b2ca52425233729707a9da Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sun, 13 Dec 2020 01:13:17 +0100
Subject: [PATCH] chang kinect wuerfel configfinder to work with achterbahn

---
 .../autoconfigurator/autoconfigurator.pde     | 605 ++++++++++++++++++
 1 file changed, 605 insertions(+)
 create mode 100644 achterbahnconfig/autoconfigurator/autoconfigurator.pde

diff --git a/achterbahnconfig/autoconfigurator/autoconfigurator.pde b/achterbahnconfig/autoconfigurator/autoconfigurator.pde
new file mode 100644
index 0000000..5c5d9e3
--- /dev/null
+++ b/achterbahnconfig/autoconfigurator/autoconfigurator.pde
@@ -0,0 +1,605 @@
+import processing.video.*; //Video | GStreamer-based video library for Processing
+
+//on linux: sudo apt-get install libgstreamer-plugins-base1.0-dev libgstreamer1.0-dev libglib2.0-dev
+//          sudo apt-get install libgstreamer-plugins-good1.0-dev
+
+import processing.serial.*;
+Serial sPort;
+
+Capture cam;
+
+PImage cam_reference;
+PImage cam_currentbox;
+PGraphics canvas;
+
+PImage image_clear;
+
+
+int currentID=0;
+//int MAXID = 50; //inclusive
+//int MAXID = 599;
+int MAXID = 10;
+int searchstate=0; //0=get reference image and ping box, 1=wait, 2=get image to compare and turn box off, 3=wait
+long starttime_wait=0;
+
+float PIXELDISTANCE_CM=1.6; //pixeldistance in centimeter
+
+boolean running=false;
+
+color testcolor=color(255,255,255);
+color offcolor=color(0,0,0);
+color successcolor=color(0,255,0);
+color failcolor=color(255,0,0);
+
+static int boxnum=19; //how many boxes are visible?
+ArrayList<Box> boxes=new ArrayList<Box>();
+//result will be written to the following arrays
+//int boxid[] = new int[boxnum]; //for found boxes
+//PVector boxpos[] = new PVector[boxnum]; //center position of box
+//PVector boxsize[] = new PVector[boxnum]; //size of box (width,height)
+
+//Settings
+//int FINDBOXES_MINIMUMBRIGHTNESS; //brightness difference required
+int FINDBOXES_MAXIMUMPIXELDISTANCE; //distance to add pixel to existing box
+PVector MINBOXSIZE; //minimum box size to accept box
+int TIME_FIND = 100; //time in ms to find box in image
+int TIME_RESULT = 100; //time in ms to show success/fail
+int TIME_TURNOFF = 100; //time in ms to wait to turn off
+float MINBRIGHTNESS_PERCENTILE=0.98; //percentile for minbrightness calculation (how many pixels will be considered). between 0(all) and 1(none). 0=all pixels. ->1=only bright pixels 
+int MINIMUMALLOWEDBRIGHTNESS=50; //minimum brightness. will be limited to this lower bound if no box in image (percentile calculation yields low value)
+
+void setup() {
+  size(640, 480);
+  frameRate(30);
+
+  /*
+  for (int i=0; i<Serial.list().length; i++) {
+    println(""+i+": "+Serial.list()[i]);
+  }
+  */
+
+  //String portName = Serial.list()[0];
+  String portName="/dev/ttyUSB0";
+  sPort = new Serial(this, portName, 115200);
+
+
+  canvas = createGraphics(width, height);
+  
+  //FINDBOXES_MINIMUMBRIGHTNESS=60; //brightness difference required
+  FINDBOXES_MAXIMUMPIXELDISTANCE=width/200; //distance to add pixel to existing box
+  MINBOXSIZE = new PVector(width/300,width/300); //minimum box size to accept box
+
+  String[] cameras = Capture.list();
+
+  if (cameras.length == 0) {
+    println("There are no cameras available for capture.");
+    exit();
+  } else {
+    println("Available cameras:");
+    for (int i = 0; i < cameras.length; i++) {
+      println(i+"->"+cameras[i]);
+    }
+
+    // The camera can be initialized directly using an
+    // element from the array returned by list():
+    //cam = new Capture(this, cameras[1]); //windows
+    cam = new Capture(this, cameras[0]); //linux
+    cam.start();
+  }
+  image_clear=cam.copy();
+  cam_reference=cam.copy();
+}
+
+void draw() {
+
+  if (cam.available() == true) {
+    cam.read();
+  }
+  //image(cam, 0, 0);
+  // The following does the same, and is faster when just drawing the image
+  // without any additional resizing, transformations, or tint.
+  //set(0, 0, cam);
+
+  //Subtract images
+  canvas.beginDraw();
+  canvas.background(0);
+  canvas.blendMode(NORMAL);
+  canvas.image(cam, 0, 0);
+  canvas.blendMode(SUBTRACT);
+  canvas.image(cam_reference, 0, 0);
+  canvas.endDraw();
+
+  image(canvas,0,0); //show camera image
+
+  if (running) {
+    checkState();
+  }
+
+  textSize(12);
+  fill(0,255,150); //textcolor
+  text("id: " + currentID, 10, 20);
+  text("state: " + searchstate, 10, 20*2);
+  
+  //draw boxes
+  for (Box box : boxes) {
+    PVector boxcenter = box.getBoxCenter();
+    PVector boxsize = box.getBoxSize();
+    
+    rectMode(CENTER);
+    noFill();
+    fill(255,255,0,100);
+    stroke(255,0,0);
+    rect(boxcenter.x,boxcenter.y, boxsize.x,boxsize.y);
+    textSize(12);
+    fill(0,255,150); //textcolor
+    text(box.getBoxID(), boxcenter.x,boxcenter.y);
+  }
+}
+
+void checkState() {
+  switch(searchstate) {
+    case 0: //0=get reference image and ping box
+    
+      println("Set achterbahn to debug mode");
+      String writeserial="debug_dark\n";
+      sPort.write(writeserial);
+      delay(500);
+      
+    
+      cam_reference=cam.copy();
+      println();
+      println("Searching BoxID "+str(currentID));
+
+      
+      colorBox(currentID);
+
+      starttime_wait=millis();
+      searchstate++;
+      break;
+
+    case 1: //1=wait
+      if (millis()>starttime_wait + TIME_FIND) { //wait some time
+          searchstate++;
+      }
+      break;
+
+    case 2: //2=get image to compare and turn box off
+      cam_currentbox=canvas.copy();
+
+      Box box = findSingleBox(cam_currentbox); //find single box in this image
+      if (box!=null && (box.getBoxSize().x<MINBOXSIZE.x || box.getBoxSize().y<MINBOXSIZE.y)) { //box too small
+        box=null;
+        println("  Box too small");
+      }
+      if (box!=null) { //if box found
+        PVector boxsize = box.getBoxSize();
+        float boxarea = boxsize.x * boxsize.y;
+        if (boxarea >= pow(cam_currentbox.width/40,2)) { //if box is big enough
+          box.setBoxID(currentID);
+          box.setConfidence(1.0);
+          boxes.add(box);
+        }
+      }
+      
+      if (box!=null) {
+        println("Found box");
+        println("Pos: "+box.getBoxCenter().x+", "+box.getBoxCenter().y+" | size: "+box.getBoxSize().x+", "+box.getBoxSize().y);
+        //colorBox(currentID);
+      }else {
+        println("No Box Found!");
+        //colorBox(currentID);
+      } 
+      
+      starttime_wait=millis();
+      searchstate++;
+      break;
+
+    case 3: //3=wait, color box fail or success
+      if (millis()>starttime_wait + TIME_RESULT) { //wait some time
+          colorAllOff();
+          starttime_wait=millis();
+          searchstate++;
+      }
+      break;
+      
+    case 4: //wait for box to turn off completely
+      if (millis()>starttime_wait + TIME_TURNOFF) { //wait some time
+          starttime_wait=millis();
+          searchstate++;
+      }
+      break;
+
+    case 5: //next box
+      searchstate=0;
+      currentID++;
+      if (currentID>MAXID) {
+        running=false;
+        cam_reference=image_clear; //show normal camera image
+      }
+      break;
+  }
+}
+
+void keyPressed() {
+  if (key == 'n' ){
+    cam_reference=cam.copy();
+    println("New Reference Set");
+
+  } else if (key == 10){ //ENTER
+    running=true;
+    colorAllOff(); //turn box off
+    currentID=0; //reset
+    searchstate=0;
+    boxes.clear(); //reset found boxes
+    println("Search started");
+  } else if (key == 'r' ){
+    running=false;
+    cam_reference=image_clear; //show normal camera image
+  } else if(key == 's') {
+     sendHeights(); 
+  } else if(key == 'S') {
+     saveToEEPROM();
+  } else if(key == 'p') {
+    calculateConfidences();
+  }
+  
+  
+
+}
+
+int getPercentileBrightness(PImage img, float percentile) { //percentile between 0 and 1
+  percentile=min(max(percentile,0.0),1.0); //limit to 0 to 1
+  int[] brightness=new int[img.pixels.length];
+  for (int i=0;i<brightness.length;i++) {
+    brightness[i] = int(brightness(img.pixels[i]));
+  }
+  int[] sortedbrightness=sort(brightness);
+  return sortedbrightness[int(percentile*sortedbrightness.length)];
+}
+
+ArrayList<Box> findBoxes(PImage img) {
+  ArrayList<Box> foundboxes=new ArrayList<Box>();
+  int minimumbrightness=getPercentileBrightness(img, MINBRIGHTNESS_PERCENTILE); //calculate brightness by percentile based on minimum box size
+  
+  minimumbrightness=max(minimumbrightness, MINIMUMALLOWEDBRIGHTNESS); //limit lower value 
+  
+  
+  println(" minimumbrightness="+minimumbrightness);
+  for (int y=0;y<img.height;y++) {
+      for (int x=0;x<img.width;x++) {
+        color pcolor = img.get(x,y);
+        
+        if (brightness(pcolor)>minimumbrightness) { //pixel has changed
+            Box nearestBox=null;
+            float nearestBoxDistance=MAX_FLOAT;
+            for (Box cb : foundboxes) { //check all known boxes
+                float cdist = cb.getDistanceTo(new PVector(x,y));
+                if (cdist<=FINDBOXES_MAXIMUMPIXELDISTANCE && cdist<nearestBoxDistance) { //is near this box and closer than to another box before
+                  nearestBox=cb; //remeber box
+                  nearestBoxDistance=cdist; //remember distance
+                }
+            }
+            if (nearestBox==null) { //no box near this pixel
+              Box newbox=new Box(); //create new box
+              newbox.addPixel(new PVector(x,y));
+              foundboxes.add(newbox); //add
+            } else {
+              nearestBox.addPixel(new PVector(x,y)); //add this pixel to near box
+            }
+        }
+      }
+    }
+    
+    println(" findBoxes(): foundboxes.size="+foundboxes.size());
+    //merge overlapping boxes together
+    for (int i=0;i<foundboxes.size();i++) {
+       Box fbox = foundboxes.get(i); 
+       if (fbox.getMarked()) { continue; } //skip if this box was marked
+       for (int i2=i+1;i2<foundboxes.size();i2++) {
+         Box fbox2 = foundboxes.get(i2);
+         if (fbox.getMarked()) { continue; } //skip if this box was marked
+         if (fbox2.isInside(fbox) && fbox.isInside(fbox2)) {
+           fbox.merge(fbox2); //add pixels to one box
+           fbox2.setMarked(true); //mark for deletion
+         }
+       
+       }
+    }
+    for (int i=0;i<foundboxes.size();i++) { //remove marked boxes
+      Box fbox = foundboxes.get(i); 
+      if (fbox.getMarked()) { //if marked
+       foundboxes.remove(fbox); //remove merged box
+       i--;
+      }
+      
+    }
+    
+    println(" after merge: foundboxes.size="+foundboxes.size());
+    return foundboxes;
+}
+
+Box findSingleBox(PImage img) {
+  ArrayList<Box> foundboxes = findBoxes(img);
+  Box mostPixelBox=null;
+  for (Box box : foundboxes) {
+    if (mostPixelBox == null || mostPixelBox.getPixelNumber()<box.getPixelNumber()) { //found box with more pixels
+      mostPixelBox=box;
+    }
+  }
+  return mostPixelBox;
+}
+
+public class Box {
+   ArrayList<PVector> boxpixels;
+   int boxid;
+   boolean marked=false;
+   float confidence=0;
+   
+   public Box() {
+     boxpixels = new ArrayList<PVector>();
+   }
+   public void setMarked(boolean pm) {
+      marked=pm; 
+   }
+   public boolean getMarked() {
+      return marked; 
+   }
+   
+   public void addPixel(PVector newpix) {
+     boxpixels.add(newpix); 
+   }
+   
+   public ArrayList<PVector> getPixels() {
+     return boxpixels;
+   }
+   
+   public void setBoxID(int pid) {
+     this.boxid=pid;
+   }
+   public int getBoxID() {
+     return this.boxid;
+   }
+   
+   public float getDistanceTo(PVector pix) { //get distance from pix to nearest point of this box
+     float mindist=MAX_FLOAT;
+     for (PVector pbox : boxpixels) {
+       float currentdist = sqrt(pow(pix.x-pbox.x,2)+pow(pix.y-pbox.y,2)); //calculate distance
+       mindist=min(currentdist,mindist); //save new minimum
+     }
+     return mindist;
+   }
+   
+   public PVector getMeanPos(){
+     PVector meanvec= new PVector(0,0);
+     for (PVector pbox : boxpixels) {
+      meanvec.x+=pbox.x;
+      meanvec.y+=pbox.y;
+     }
+     meanvec.x/=boxpixels.size();
+     meanvec.y/=boxpixels.size();
+     return meanvec;
+   }
+   
+   public PVector getBoxSize(){ //returns widht and height as vector
+     PVector upperleft= new PVector(MAX_FLOAT,MAX_FLOAT); //min x and y
+     PVector lowerright= new PVector(0,0); //max x and y
+     for (PVector pbox : boxpixels) {
+       upperleft.x=min(upperleft.x, pbox.x);
+       upperleft.y=min(upperleft.y, pbox.y);
+       
+       lowerright.x=max(lowerright.x, pbox.x);
+       lowerright.y=max(lowerright.y, pbox.y);
+     }
+     PVector boxsize = new PVector(lowerright.x-upperleft.x, lowerright.y-upperleft.y);
+     return boxsize;
+   }
+   
+   public PVector getBoxCenter(){ //returns width and height as vector
+     PVector upperleft= new PVector(MAX_FLOAT,MAX_FLOAT); //min x and y
+     PVector lowerright= new PVector(0,0); //max x and y
+     for (PVector pbox : boxpixels) {
+       upperleft.x=min(upperleft.x, pbox.x);
+       upperleft.y=min(upperleft.y, pbox.y);
+       
+       lowerright.x=max(lowerright.x, pbox.x);
+       lowerright.y=max(lowerright.y, pbox.y);
+     }
+     PVector boxcenter = new PVector((lowerright.x+upperleft.x)/2, (lowerright.y+upperleft.y)/2);
+     return boxcenter;
+   }
+   
+   public int getPixelNumber()  {
+      return this.boxpixels.size(); 
+   }
+   
+   public boolean isInside(Box pbox) {
+     int pixelsinside=0; //for counting pixels of pbox inside this box
+     PVector pboxsize=pbox.getBoxSize();
+     PVector pboxcenter=pbox.getBoxCenter();
+     float pboxX1 = pboxcenter.x-pboxsize.x/2;
+     float pboxX2 = pboxcenter.x+pboxsize.x/2;
+     float pboxY1 = pboxcenter.y-pboxsize.y/2;
+     float pboxY2 = pboxcenter.y+pboxsize.y/2;
+     for (PVector pix : boxpixels) {
+         if (pix.x>pboxX1 && pix.x<pboxX2 && pix.y>pboxY1 && pix.y<pboxY2) { //is inside
+           pixelsinside++;
+         }
+     }
+     
+     return pixelsinside>0; 
+     
+   }
+   
+   public void merge(Box mergebox) { //merge pixels from mergebox inside this box
+     boxpixels.addAll(mergebox.getPixels());
+   }
+   
+   public float getConfidence() {
+     return confidence;
+   }
+   public void setConfidence(float pconf) {
+     confidence=pconf;
+   }
+}
+
+void colorBox(int id) 
+{
+  //sPort.write("B,"+str(id)+","+str(0)+","+int(red(c))+","+int(green(c))+","+int(blue(c))+"\n");
+  String writeserial="px="+str(id)+"\n";
+    sPort.write(writeserial);
+  delay(50);
+}
+void colorAllOff() 
+{
+  String writeserial="px="+str(-1)+"\n"; //value outside range doesnt show
+    sPort.write(writeserial);
+  println("writeserial="+writeserial);
+  delay(50);
+}
+
+void sendHeights(){
+  float pixelscale=PIXELDISTANCE_CM/getMeanDist();
+  println("pixelscale="+pixelscale);
+  float minHeightValue=10000;
+  float maxHeightValue=0;
+  for (int i=0;i<boxes.size();i++) {
+    Box box=boxes.get(i);
+    PVector boxcenter=box.getBoxCenter();
+    minHeightValue=min(pixelscale*boxcenter.y,minHeightValue);
+    maxHeightValue=max(pixelscale*boxcenter.y,maxHeightValue);
+  }
+  println("minHeightValue="+minHeightValue+", maxHeightValue="+maxHeightValue);
+  if(maxHeightValue-minHeightValue>=255) {
+    println("WARNING. Too much height difference!"); 
+  }
+  
+  println("clear Heightmap");
+  String writeserial="clear\n";
+    sPort.write(writeserial);
+  delay(200);
+  
+  for (int i=0;i<boxes.size();i++) {
+    Box box=boxes.get(i);
+    PVector boxcenter=box.getBoxCenter();
+    float conf=box.getConfidence();
+    //boxconfig[i]=+";"+(boxcenter.x/cam.width)+";"+(boxcenter.y/cam.height)+";"+(boxsize.x/cam.width)+";"+(boxsize.y/cam.height);
+    float pixelvalue = pixelscale*boxcenter.y-minHeightValue; //0 to 255
+    
+    pixelvalue=constrain(int(pixelvalue),0,255);
+    
+    if (conf>0.5) {
+    writeserial="setpx="+box.getBoxID()+","+int(pixelvalue)+"\n";
+      sPort.write(writeserial);
+    }
+  }
+  println("send");
+}
+
+void calculateConfidences() {
+  for (int i=0;i<boxes.size();i++) {
+    Box box=boxes.get(i);
+    int boxid=box.getBoxID();
+    PVector boxcenter=box.getBoxCenter();
+    PVector boxsize=box.getBoxSize();
+    float boxarea = boxsize.x * boxsize.y;
+    
+    //calculate distance to neighboring boxes
+    int prevBoxi=-1;
+    int nextBoxi=-1;
+    for (int j=0;j<boxes.size();j++) {
+       if (boxes.get(j).getBoxID()<boxid) { //found
+         if (prevBoxi<0 || boxes.get(j).getBoxID()>boxes.get(prevBoxi).getBoxID()) { //new one closer to boxid
+           prevBoxi=j;
+         }
+       }
+       if (boxes.get(j).getBoxID()>boxid) { //found
+         if (nextBoxi<0 || boxes.get(j).getBoxID()<boxes.get(nextBoxi).getBoxID()) { //new one closer to boxid
+           nextBoxi=j;
+         }
+       }
+    }
+    float prevdist=0;
+    if (prevBoxi>=0) {
+      PVector prevboxcenter=boxes.get(prevBoxi).getBoxCenter();
+      prevdist=dist(prevboxcenter.x,prevboxcenter.y,boxcenter.x,boxcenter.y);
+      int boxsteps = boxid-boxes.get(prevBoxi).getBoxID(); //1 if previous box found. greater if boxes missing in between
+      prevdist*=boxsteps;
+    }
+    float nextdist=0;
+    if (nextBoxi>=0) {
+      PVector nextboxcenter=boxes.get(nextBoxi).getBoxCenter();
+      nextdist=dist(nextboxcenter.x,nextboxcenter.y,boxcenter.x,boxcenter.y);
+      int boxsteps = boxes.get(nextBoxi).getBoxID()-boxid; //1 if previous box found. greater if boxes missing in between
+      prevdist*=boxsteps;
+    }
+    float maxdist=max(prevdist,nextdist); //maximum distance to neighbor if one exists
+    maxdist/=cam.width;
+    
+    float confidence=constrain(map(maxdist, 0.05, 0.5, 1.0,0.1), 0.1,1.0);
+    
+    
+    confidence*=constrain(map(boxarea, 1000, 10000, 1.0,0.5), 0.5,1.0);
+    
+    box.setConfidence(confidence);
+    println(boxid+":"+"maxdist="+maxdist+", area="+boxarea+", confidence="+confidence);
+  }
+}
+
+float getMeanDist() { //run after confidences have been calculated
+  float meandist=0;
+  int meancount=0;
+  for (int i=0;i<boxes.size();i++) {
+    Box box=boxes.get(i);
+    float mindist=-1;
+    if (box.getConfidence()>0.5) { //confidence high enough
+      int boxid=box.getBoxID();
+      PVector boxcenter=box.getBoxCenter();
+      PVector boxsize=box.getBoxSize();
+      
+      //calculate distance to neighboring boxes
+      int prevBoxi=-1;
+      int nextBoxi=-1;
+      for (int j=0;j<boxes.size();j++) {
+         if (boxes.get(j).getBoxID()<boxid) { //found
+           if (prevBoxi<0 || boxes.get(j).getBoxID()>boxes.get(prevBoxi).getBoxID()) { //new one closer to boxid
+             prevBoxi=j;
+           }
+         }
+         if (boxes.get(j).getBoxID()>boxid) { //found
+           if (nextBoxi<0 || boxes.get(j).getBoxID()<boxes.get(nextBoxi).getBoxID()) { //new one closer to boxid
+             nextBoxi=j;
+           }
+         }
+      }
+      float prevdist=0;
+      if (prevBoxi>=0) {
+        PVector prevboxcenter=boxes.get(prevBoxi).getBoxCenter();
+        prevdist=dist(prevboxcenter.x,prevboxcenter.y,boxcenter.x,boxcenter.y);
+        int boxsteps = boxid-boxes.get(prevBoxi).getBoxID(); //1 if previous box found. greater if boxes missing in between
+        prevdist*=boxsteps;
+      }
+      float nextdist=0;
+      if (nextBoxi>=0) {
+        PVector nextboxcenter=boxes.get(nextBoxi).getBoxCenter();
+        nextdist=dist(nextboxcenter.x,nextboxcenter.y,boxcenter.x,boxcenter.y);
+        int boxsteps = boxes.get(nextBoxi).getBoxID()-boxid; //1 if previous box found. greater if boxes missing in between
+        prevdist*=boxsteps;
+      }
+      mindist=max(prevdist,nextdist); //maximum distance to neighbor if one exists
+    }
+    
+    if (mindist>0){
+      meancount++;
+      meandist+=mindist;
+    }
+    
+  } //end for all boxes
+  return meandist/meancount;
+}
+
+void saveToEEPROM() {  
+  String writeserial="save\n";
+    sPort.write(writeserial);
+  delay(500);
+  println("send save to eeprom");
+}