Kinect 3: Depth Tracking + Rhino

 

The definition above can be downloaded here: surface_pts.gh

import java.nio.*;
import KinectPV2.*;
import oscP5.*;
import netP5.*;

import controlP5.*;

NetAddress myBroadcastLocation; 
OscMessage myMessage;
OscP5 oscP5;

ControlP5 cp5;

KinectPV2 kinect;


int  vertLoc;

//transformations
float a = 1.9;
int zval = -200;
float scaleVal = 1;


//value to scale the depth point when accessing each individual point in the PC.
float scaleDepthPoint = 100.0;

//Distance Threashold
int maxD = 4000; // 4m
int minD = 0;  //  0m

int Ztranslate = 0;

//openGL object and shader
PGL     pgl;
PShader sh;

//VBO buffer location in the GPU
int vertexVboId;


ArrayList points = new ArrayList();
ArrayList zpoints = new ArrayList();

int oldx = mouseX;
int oldy = mouseY;
float rotx = PI;
float roty = 0;
float zcam = 500;
int initialize = 0;
int xstart;
int xclamp;
int ystart;
int yclamp;

boolean points_on = true;
boolean send_data = false;

PImage img;


public void setup() {
  size(1280, 720, P3D);
  perspective(PI/3.0,(float)width/height,1,100000);
  
  oscP5 = new OscP5(this,6880);
  myBroadcastLocation = new NetAddress("localhost",1200);
  kinect = new KinectPV2(this);

  kinect.enableDepthImg(true);

  kinect.enablePointCloud(true);

  kinect.setLowThresholdPC(minD);
  kinect.setHighThresholdPC(maxD);

  kinect.init();
  

  sh = loadShader("frag.glsl", "vert.glsl");
  
  
  cp5 = new ControlP5(this);
  
  cp5.addSlider("minD")
     .setPosition(40,40)
     .setRange(0,4000)
     .setSize(240,20)
     .setValue(0)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
    ;
    
  cp5.addSlider("maxD")
     .setPosition(40,80)
     .setRange(0,14000)
     .setSize(240,20)
     .setValue(4000)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
    ;
    
  cp5.addSlider("Ztranslate")
     .setPosition(40,120)
     .setRange(0,1000)
     .setSize(240,20)
     .setValue(0)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
    ;
    
  cp5.addToggle("points_on")
     .setPosition(40,160)
     .setSize(20,20)
     .setColorForeground(color(20,20,20))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(0xffff88ff)
     .setColorActive(color(0,200,200))
     ;
  
  cp5.addToggle("send_data")
     .setPosition(80,160)
     .setSize(20,20)
     .setColorForeground(color(20,20,20))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(0xffff88ff)
     .setColorActive(color(0,200,200))
     ;
    
  cp5.addSlider("xstart")
     .setPosition(340,40)
     .setRange(-200,200)
     .setSize(240,20)
     .setValue(-18)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
    ;
    
  cp5.addSlider("xclamp")
     .setPosition(340,80)
     .setRange(0,100)
     .setSize(240,20)
     .setValue(46)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
    ;
    
  cp5.addSlider("ystart")
     .setPosition(340,120)
     .setRange(-200,200)
     .setSize(240,20)
     .setValue(-18)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
    ;
    
  cp5.addSlider("yclamp")
     .setPosition(340,160)
     .setRange(0,100)
     .setSize(240,20)
     .setValue(46)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
    ;
    
    

   

  PGL pgl = beginPGL();

  IntBuffer intBuffer = IntBuffer.allocate(1);
  pgl.genBuffers(1, intBuffer);

  //memory location of the VBO
  vertexVboId = intBuffer.get(0);

  endPGL();
}

public void draw() {
  background(0);
  myMessage = new OscMessage("/points");
   
  pushMatrix();
  cam();

  //draw the depth capture images


  //translate the scene to the center
  translate(0,0,-Ztranslate);
  scale(scaleVal,  -1 * scaleVal, scaleVal);


  // Threahold of the point Cloud.
  kinect.setLowThresholdPC(minD);
  kinect.setHighThresholdPC(maxD);

  //get the points in 3d space
  FloatBuffer pointCloudBuffer = kinect.getPointCloudDepthPos();
  
  points = new ArrayList();
  zpoints = new ArrayList();

  // obtain XYZ the values of the point cloud  
  for(int i = 0; i < kinect.WIDTHDepth * kinect.HEIGHTDepth; i+=3){ float x = pointCloudBuffer.get(i*3 + 0) * scaleDepthPoint; float y = pointCloudBuffer.get(i*3 + 1) * scaleDepthPoint; float z = pointCloudBuffer.get(i*3 + 2) * scaleDepthPoint; if(points_on == true){ stroke(255); }else{ stroke(0); } if((x > xstart && x < xstart + xclamp) && (y > ystart && y < ystart+yclamp) && x != 0) {
        stroke(0,255,255);
        points.add(new PVector(x,y,z));
      }
      strokeWeight(2);
      point(x, y, z);
   }
   int sp = 3;
   int part = (xclamp)/sp+1;
   myMessage.add(part);
   println(part);
   for (int xx = xstart; xx < xstart + xclamp;){
     for (int yy = ystart; yy < ystart + yclamp;){
       int clz = 0;
       float mind = 99999;
       for(int i = 0; i < points.size(); i++){
         PVector pt = points.get(i);
         if(dist(xx,yy,pt.x,pt.y) < mind){
           mind = dist(xx,yy,pt.x,pt.y);
           clz = i; 
         }
       }
       if(mind != 99999){
         PVector fpt = points.get(clz);
         zpoints.add(new PVector(xx,yy,fpt.z));
       }
       yy = yy +sp;
     }
     xx = xx + sp;
   }
   
   for(int i = 0; i < zpoints.size(); i++){ PVector pt = zpoints.get(i); stroke(255,0,0); strokeWeight(3); point(pt.x,pt.y, pt.z); if(send_data == true ){ myMessage.add(pt.x); myMessage.add(pt.y); myMessage.add(pt.z); } } println(zpoints.size()); //begin openGL calls and bind the shader popMatrix(); oscP5.send(myMessage, myBroadcastLocation); } void cam() { int newx = mouseX; int newy = mouseY; translate(width/2, height/2,zcam); rotateY(rotx); rotateX(roty); translate(0,0,-30); //rotateZ(PI); if ((mousePressed == true) && (mouseY > 200) ) {
    rotx = rotx + (oldx-newx)/50.0;
    roty = roty + (oldy-newy)/50.0;
  }
  
  if(initialize == 0){
     rotx = rotx + (oldx-newx)/50.0;
    roty = roty + (oldy-newy)/50.0;
    initialize = 1;
  }
    

  
  oldx = newx;
  oldy = newy;
}

void mouseWheel(MouseEvent event) {
  float e = event.getCount();
  zcam = zcam - e*5;
}