Kinect 2: Depth Tracking + Blob Detection

import controlP5.*;
import KinectPV2.*;

import blobDetection.*;

KinectPV2 kinect;

ControlP5 cp5;

PImage img;
PImage imgc;

float xpos = 0;
float ypos = 0;

float k = 0.02;

String vsend;

BlobDetection theBlobDetection;


int maxD = 4000; // 4m
int minD = 100;  //  0m

int barea = 200; 

PGraphics pg;


boolean depth_on = true;
boolean blob_on = true;
boolean bounding_on = true;
boolean tracking_on = true;
  
IntList ptrack;

ArrayList<PVector> pos = new ArrayList<PVector>();
int trn = 0;

void setup() {
  size(1024, 848);
  
  pg = createGraphics(512,424);

  kinect = new KinectPV2(this);
  kinect.enableDepthImg(true);
  kinect.enablePointCloud(true);
  kinect.enableColorImg(true);
  
  kinect.init();
  
  cp5 = new ControlP5(this);
  
  cp5.addSlider("maxD")
    .setPosition(50,630)
    .setRange(0,14000)
    .setSize(200,20)
    .setValue(1330)
    .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("barea")
    .setPosition(50,660)
    .setRange(0,500)
    .setSize(200,20)
    .setValue(100)
    .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("depth_on")
     .setPosition(50,700)
     .setSize(20,20)
     .setValue(true)
     .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("blob_on")
     .setPosition(50,740)
     .setSize(20,20)
     .setValue(true)
     .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("bounding_on")
     .setPosition(50,780)
     .setSize(20,20)
     .setValue(true)
     .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("tracking_on")
     .setPosition(210,780)
     .setSize(20,20)
     .setValue(true)
     .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
     ;
  
  
  img = new PImage(512, 424); 
  
  theBlobDetection = new BlobDetection(img.width, img.height);
  theBlobDetection.setPosDiscrimination(true);
  theBlobDetection.setThreshold(0.1f); 
}

void draw() {
  background(0);
  
  ptrack = new IntList();
  
  kinect.setLowThresholdPC(minD);
  kinect.setHighThresholdPC(maxD);
  
  img = kinect.getPointCloudDepthImage();
  //img.filter(INVERT);
  
  pg.beginDraw();
  pg.background(0);
  //pg.rotate(.01);
  pg.image(img,0,0);
  pg.endDraw();
  
  if(depth_on == true){
    image(pg,512,0);
  }

  theBlobDetection.computeBlobs(pg.pixels);
  drawBlobsAndEdges(bounding_on,blob_on);
  
  if(tracking_on == true){
    for(int i = 0; i < ptrack.size()-1; i++){
      int xp = ptrack.get(i);
      int yp = ptrack.get(i+1);
      i++;
      fill(240,20,70);
      noStroke();
      ellipse(xp*2, yp*2, 20,20);
      println(xp);
    }
  }

  fill(255);
  text(frameRate, width-100, height - 50);
  if(trn == 1){
    trace();
  }

}


// ==================================================
// drawBlobsAndEdges()
// ==================================================
void drawBlobsAndEdges(boolean drawBlobs, boolean drawEdges)
{
  noFill();
  Blob b;
  EdgeVertex eA,eB;
  xpos = 0;
  ypos = 0;
  int c = 0;
  for (int n=0 ; n<theBlobDetection.getBlobNb() ; n++)
  {
    b=theBlobDetection.getBlob(n);
    if (b!=null){
      // Edges
      if (drawEdges){
        strokeWeight(1);
        stroke(0,255,255);
        for (int m=0;m<b.getEdgeNb();m++) { 
          eA = b.getEdgeVertexA(m); 
          eB = b.getEdgeVertexB(m); 
          if (eA !=null && eB !=null) 
            line( eA.x*img.width, eA.y*img.height, eB.x*img.width, eB.y*img.height ); 
        } 
      } 
// Blobs 
      if (true) { 
        if(b.w*img.width > barea && b.h*img.height > barea  ){
          strokeWeight(1);
          float cx = ((b.xMin*img.width) + b.w*img.width/2);
          float cy = ((b.yMin*img.height)+b.h*img.height/2);
          noStroke();
          if (drawBlobs){
            stroke(255,0,0);
            strokeWeight(1);
rect(b.xMin*img.width,b.yMin*img.height,b.w*img.width,b.h*img.height);
          fill(255);
          noStroke();
          ellipse(cx,cy,10,10);
        }
          c++;
          ptrack.append(int(cx));
          ptrack.append(int(cy));
          if(dist(cx,cy,img.width/2,img.height/2) < dist(xpos,ypos,img.width/2,img.height/2)){
              xpos = cx;
              ypos = cy;
            }
            noFill();
          }
      }
    }

      }
      ptrack.append(c);
}

void trace(){
  int xp = 0;
  int yp = 0;
  for(int i = 0; i < ptrack.size()-1; i++){
      xp = ptrack.get(i);
      yp = ptrack.get(i+1);
      i++;
    }
  pos.add(new PVector(xp*2,yp*2,0));
  for(int i = 0; i < pos.size()-1; i++){
      PVector fp =  pos.get(i);
      PVector lp =  pos.get(i+1);
      strokeWeight(10);
      stroke(255,100);
      noFill();
      line( fp.x,fp.y,lp.x,lp.y);
    }
}

void keyPressed() {
  if (key == ENTER) {
    if(trn == 1){
      pos = new ArrayList();
    }
    if(trn == 0){
      trn = 1;
    }
  }
}