chang kinect wuerfel configfinder to work with achterbahn

This commit is contained in:
interfisch 2020-12-13 01:13:17 +01:00
parent ee1ed34935
commit 7e4c5352b3
1 changed files with 605 additions and 0 deletions

View File

@ -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");
}