kinect_3d_point_cloud.java

gistfile1.txt

import shiffman.kinect.*;
import processing.opengl.*;
import peasy.*;

PImage depth;

float tolerance = 5;
float targetDepth = 0;
int skip = 5;

boolean showImage = true;

PeasyCam cam;

void setup() {
  size(640,480, OPENGL);
  NativeKinect.init();

  depth = createImage(640,480,RGB);
  lights();
  background(255);
    cam = new PeasyCam(this, width);
 // sphereDetail(5);

}

static final int gray(color value) { 
  return max((value >> 16) & 0xff, (value >> 8 ) & 0xff, value & 0xff);
} 

void draw() {
  NativeKinect.update();

  depth.pixels = NativeKinect.getDepthMap();
  depth.updatePixels();


  if(showImage){
    image(depth,-width/2,-height/2,640,480);
  } else {
    background(255);
    render3d();
  }
 
}

void drawSphere(float x, float y, float d) {

  
  //float di = depthInInches(d);

  
  x = (x - 320) * d/603.3;
  y = (y - 240) * d/603.3;


  
   float z = map(d, 0, 255, -255, 0);
       //println(x + "," + y + "," + z);
   x = map(x, -20, 20, 0 - width/2,  width/2);
    y = map(y, -20, 20, 0 - height/2, height/2);
 // pushMatrix();
    strokeWeight(3);
    point(x,y,z);
   // translate(x, y, z);
    //sphere(3);
 // popMatrix();
  
}


float depthInInches(float input) {
  return depthInMeters(input) / 0.0254;
}

float depthInMeters(float rawDepth) {

  float a = 0.0056748 ;
  float b = 0.000083708165 ;

  float ccc = 0.6084244 ;

  return pow(a +b*rawDepth,-1.0/ccc) / 1000;
}


void render3d(){
 for(int x = 0; x < 640; x+=skip) {
    for(int y = 0; y < 480; y+=skip) {
      int loc = x + y * depth.width;
      
      drawSphere(float(x),float(y),gray(depth.pixels[loc]));
    }
  }
}


void mouseClicked() {
  showImage = !showImage;
}