working kinect to box implementation
This commit is contained in:
Philipp Kramer
commit
fa43303ae7
1 changed files with 408 additions and 0 deletions
408
kinectv1_wuerfel.pde
Normal file
408
kinectv1_wuerfel.pde
Normal file
|
|
@ -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";
|
||||
|
||||
}
|
||||
Loading…
Reference in a new issue