Kinect 6: Object Detection + Processing

import controlP5.*;
import KinectPV2.*;
import blobDetection.*;
BlobDetection theBlobDetection;

KinectPV2 kinect;

ControlP5 cp5;

int KmaxD = 4000; // 4m
int KminD = 100;  //  0m

int MoveX; 
int MoveY;

float Scale_Kinect;
boolean kinect_on = false;

ArrayList grid = new ArrayList();
ArrayList flowpts = new ArrayList();

IntList trkpts = new IntList();

int num = 8;

int[] lpts = new int[num];
int[] dia = new int[num];
int[] pts = new int[num];


float mx = 0;
float my  = 0;
float sx = 99999;
float sy  = 99999;

PImage img;
PImage Kimg;
PGraphics pg;

int xsize = 70;
int ysize = 35;
int space = 35;

int bg = 0;

int blob_size;


void setup() {
  //size(1600, 1000);
  fullScreen();
  smooth(4);
   pg = createGraphics(512, 424);
  
   cp5 = new ControlP5(this);
  
   kinect = new KinectPV2(this);
   kinect.enableDepthImg(true);
   kinect.enablePointCloud(true);
   kinect.enableColorImg(true);
  
  kinect.init();
  
  theBlobDetection = new BlobDetection(512, 424);
  theBlobDetection.setPosDiscrimination(true);
  theBlobDetection.setThreshold(0.1f); 
  
  cp5 = new ControlP5(this);
  
  cp5.addToggle("kinect_on")
     .setPosition(40,40)
     .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("KminD")
    .setPosition(100,40)
    .setRange(0,700)
    .setSize(200,20)
    .setValue(200)
    .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("KmaxD")
    .setPosition(100,80)
    .setRange(0,700)
    .setSize(200,20)
    .setValue(450)
    .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("Scale_Kinect")
    .setPosition(340,40)
    .setRange(0,5)
    .setSize(200,20)
    .setValue(5)
    .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("blob_size")
    .setPosition(340,80)
    .setRange(0,40000)
    .setSize(200,20)
    .setValue(8000)
    .setColorForeground(color(20,200,200))
     .setColorLabel(color(255))
     .setColorBackground(color(70,70,70))
     .setColorValue(color(0,0,0))
     .setColorActive(color(0,255,255))
  ;
  

  
  img = loadImage("heat.jpg");
 
  for(int i = 0; i < xsize; ++i){
    for(int j = 0; j < ysize; j++){
      float xx = i*space + (width/2 - xsize*space/2);
      float yy = j*space + (height/2 - ysize*space/2);
      float zz = 0;
      grid.add(new PVector(xx,yy,zz));
      flowpts.add(new PVector(xx,yy,zz));
      if(xx > mx){
        mx = xx;
      }
      if(yy > my){
        my = yy;
      }
      if(xx < sx){
        sx = xx;
      }
      if(yy < sy){
        sy = yy;
      }
    }
  }

  for(int i = 0; i < 0; i++){
    lpts[i] = int(random(grid.size()-1));
    trkpts.append(int(random(grid.size()-1)));
  }
  
  //noLoop();
  background(30);
}


void draw() {
  if(bg == 0){
    background(30);
    bg = 1;
  }
  
  kinect.setLowThresholdPC(KminD);
  kinect.setHighThresholdPC(KmaxD);
  
  Kimg = kinect.getPointCloudDepthImage();
  
  trkpts = new IntList();
  trkpts.append(120);
  trkpts.append(450);
  
  float sc = Scale_Kinect;
  pushMatrix();
  translate(width/2,height/2);
  pg.beginDraw();
  pg.background(100);
  pg.image(Kimg,0,0);
  pg.endDraw();
  scale(sc);
  if(kinect_on){
    background(30);
    image(pg,-512/2,-424/2);
    bg = 0;
  }
  popMatrix();
  pushMatrix();
  theBlobDetection.computeBlobs(pg.pixels);
  drawBlobsAndEdges(kinect_on,kinect_on);
  popMatrix();
  
  fill(0,0,0,3);

  noStroke();
  rect(0,0,width,height);
  
  //image(img, 30, 30);
  
  for(int i = 0; i < (grid.size()); ++i){
    PVector vs =  grid.get(i);
    noStroke();
    fill(100);
    int rad = 7;
    for(int j = 0; j < trkpts.size(); j++){
      if(trkpts.get(j) == i){
        fill(0,102,153);
        rad = 30;
      }
    }
    ellipse(vs.x,vs.y,rad,rad);
  }
  
  ////////////weighted average/////////
  
  ////////////get low and high distance//////////////

  PVector vs0 =  flowpts.get(0);
  PVector vsn =  grid.get(trkpts.get(0));
  float l = 1/dist(vs0.x,vs0.y,vsn.x,vsn.y);
  float h = 1/dist(vs0.x,vs0.y,vsn.x,vsn.y);
  for(int i = 0; i < flowpts.size(); i++){
      for(int ac = 0; ac < trkpts.size(); ac++){
          vsn =  grid.get(trkpts.get(ac));
          float d = dist(vs0.x,vs0.y,vsn.x,vsn.y);
          if(d < l){
            l = d;
          }
          if(d > h){
            h = d;
        }
      }
  }

  for(int i = 0; i < flowpts.size(); i++){
    PVector vs =  flowpts.get(i);
    float dv = 0;
    float tx = 0;
    float ty = 0;
    
    
    float dd = 99999;
    for(int ac = 0; ac < trkpts.size(); ac++){
      if(ac < num){
        vsn =  grid.get(trkpts.get(ac));
      }else{
        vsn = (new PVector(mouseX,mouseY,0));
      }  
        float nd = dist(vs.x,vs.y,vsn.x,vsn.y);
        if(nd < dd){
          dd = nd;
        }
        float d = 1/dist(vs.x,vs.y,vsn.x,vsn.y);
        float rd = map(d,l,h,0.0,1.0);
        tx = tx + vsn.x*rd*rd;
        ty = ty + vsn.y*rd*rd;
        dv = dv + rd*rd;
    }
   
    tx = tx/dv;
    ty = ty/dv;
    float na = -atan2(tx-vs.x, ty-vs.y);
    stroke(0,255,255);
    //line(vs.x,vs.y,tx,ty);
    color c = img.get(int(dd),2);
    stroke(c);
    int ww = int(7-dd/22);
    if(ww < 1){
      ww = 1;
    }
    strokeWeight(ww*3);
    if(vs.x > mx || vs.x < sx){
      stroke(25);
    }
    if(vs.y > my || vs.y < sy){
      stroke(25);
    }
    point(vs.x,vs.y);
    float nx = vs.x;
    float ny = vs.y;
    
    nx = nx + 1 * cos(na);
    ny = ny + 1 *sin(na);
    flowpts.set(i,new PVector(nx,ny,0));

    pushMatrix();
    translate(vs.x,vs.y);
    rotate(na);
    stroke(255);
    //println(na);
    //line(-20,0,20,0);
    popMatrix();
    
  }
  
  if(frameCount%600 == 0){
      flowpts = new ArrayList();
      for(int i = 0; i < (grid.size()); ++i){
        PVector vs =  grid.get(i);
        flowpts.add(new PVector(vs.x,vs.y,0));
      }
    }
  
}

void mouseClicked() {
  for(int i = 0; i < (grid.size()); ++i){
    PVector vs =  grid.get(i);
    if(dist(mouseX,mouseY,vs.x,vs.y) < 9){
      fill(255,0,0);
      int[] npts = new int[num];
      for(int j = 1; j < num; j++){
        npts[j] = lpts[j-1];
      }
      lpts = npts;
      lpts[0] = i;
    }
  }
}


// ==================================================
// drawBlobsAndEdges()
// ==================================================
void drawBlobsAndEdges(boolean drawBlobs, boolean drawEdges)
{
  noFill();
  Blob b;
  EdgeVertex eA,eB;
  for (int n=0 ; n blob_size){
          float xc = b.xMin*512*Scale_Kinect+width/2-512*Scale_Kinect/2 + b.w*512*Scale_Kinect/2;
           float yc = b.yMin*424*Scale_Kinect+height/2-424*Scale_Kinect/2 + b.h*424*Scale_Kinect/2;
          if (drawBlobs){
          rect(
            b.xMin*512*Scale_Kinect+width/2-512*Scale_Kinect/2,b.yMin*424*Scale_Kinect+height/2-424*Scale_Kinect/2,
            b.w*512*Scale_Kinect,b.h*424*Scale_Kinect
            );
            
           noStroke();
           fill(255,0,0);
           ellipse(xc,yc,10,10);
          }
           for(int i = 0; i < (grid.size()); ++i){
              PVector vs =  grid.get(i);
              if(dist(xc,yc,vs.x,vs.y) < 20){
                trkpts.append(i);
              }
            }
           
        }
      }

    }

}