421 lines
12 KiB
Plaintext
421 lines
12 KiB
Plaintext
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 = 0;
|
|
int maxDepth = 910;
|
|
|
|
// What is the kinect's angle
|
|
float angle;
|
|
|
|
static int boxnum=50; //maximum boxes
|
|
int boxnumloaded=boxnum; //can be changes if less boxes are loaded from config
|
|
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=20;
|
|
//int fadetime=1000/FRAMERATE /10; //fadetime in centiseconds
|
|
int fadetime=1000/FRAMERATE /10 *4; //fadetime in centiseconds SMOOOOTH
|
|
|
|
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();
|
|
boxsize[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);
|
|
}*/
|
|
|
|
/* for testing
|
|
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<boxnumloaded;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,255); //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;
|
|
brightness=int(255-(cpmax-cpmean));
|
|
}
|
|
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);
|
|
}
|
|
|
|
}
|
|
|
|
if (editboxpos) {
|
|
box[editbox]=color(0,0,255); //highlight current edited box
|
|
}
|
|
sendToWuerfels();
|
|
|
|
textSize(12);
|
|
fill(0,255,150); //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(100,50,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') { //toggle 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 color as depth
|
|
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<boxnumloaded;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[boxnumloaded];
|
|
for (int i=0;i<boxnumloaded;i++) {
|
|
boxconfig[i]=boxid[i]+";"+(1.0*boxpos[i].x/kinect.width)+";"+(1.0*boxpos[i].y/kinect.height)+";"+(1.0*boxsize[i].x/kinect.width)+";"+(1.0*boxsize[i].y/kinect.height);
|
|
}
|
|
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;
|
|
}
|
|
boxnumloaded = boxconfig.length;
|
|
for (int i=0;i<min(boxconfig.length,boxnum);i++) {
|
|
String[] _boxconfsplit=split(boxconfig[i], ';');
|
|
boxid[i]=int(_boxconfsplit[0]);
|
|
boxpos[i].x=int(float(_boxconfsplit[1])*kinect.width);
|
|
boxpos[i].y=int(float(_boxconfsplit[2])*kinect.height);
|
|
boxsize[i].x=int(float(_boxconfsplit[3])*kinect.width);
|
|
boxsize[i].y=int(float(_boxconfsplit[4])*kinect.height);
|
|
boxenabled[i]=true;
|
|
}
|
|
println("loaded");
|
|
infomessagecounter=FRAMERATE*2;
|
|
infomessage="loaded";
|
|
|
|
}
|