OpenKinect and Processing – Can't display Z co-ordinate

Question

I have this code from Daniel Shiffman (below). I am trying to read out the Z co-ordinate. I'm not sure at all in how to do this, so any help would be much appreciated.

AveragePointTracking.pde

// Daniel Shiffman
// Tracking the average location beyond a given depth threshold
// Thanks to Dan O'Sullivan
// http://www.shiffman.net
// https://github.com/shiffman/libfreenect/tree/master/wrappers/java/processing

import org.openkinect.*;
import org.openkinect.processing.*;

// Showing how we can farm all the kinect stuff out to a separate class
KinectTracker tracker;
// Kinect Library object
Kinect kinect;

void setup() {
  size(640,600);
  kinect = new Kinect(this);
  tracker = new KinectTracker();
}

void draw() {
  background(255);

  // Run the tracking analysis
  tracker.track();
  // Show the image
  tracker.display();

  // Let's draw the raw location
  PVector v1 = tracker.getPos();
  fill(50,100,250,200);
  noStroke();
  ellipse(v1.x,v1.y,10,10);

  // Let's draw the "lerped" location
  //PVector v2 = tracker.getLerpedPos();
  //fill(100,250,50,200);
  //noStroke();
  //ellipse(v2.x,v2.y,20,20);

  // Display some info
  int t = tracker.getThreshold();
  fill(0);
  text("Location-X: " + v1.x,10,500);
  text("Location-Y: " + v1.y,10,530);
  text("Location-Z: ",10,560);
  text("threshold: " + t,10,590);
}

void stop() {
  tracker.quit();
  super.stop();
}

KinectTracker.pde

class KinectTracker {

  // Size of kinect image
  int kw = 640;
  int kh = 480;
  int threshold = 500;

  // Raw location
  PVector loc;

  // Interpolated location
  PVector lerpedLoc;

  // Depth data
  int[] depth;


  PImage display;

  KinectTracker() {
    kinect.start();
    kinect.enableDepth(true);

    // We could skip processing the grayscale image for efficiency
    // but this example is just demonstrating everything
    kinect.processDepthImage(true);

    display = createImage(kw,kh,PConstants.RGB);

    loc = new PVector(0,0);
    lerpedLoc = new PVector(0,0);
  }

  void track() {

    // Get the raw depth as array of integers
    depth = kinect.getRawDepth();

    // Being overly cautious here
    if (depth == null) return;

    float sumX = 0;
    float sumY = 0;
    float count = 0;

    for(int x = 0; x < kw; x++) {
      for(int y = 0; y < kh; y++) {
        // Mirroring the image
        int offset = kw-x-1+y*kw;
        // Grabbing the raw depth
        int rawDepth = depth[offset];

        // Testing against threshold
        if (rawDepth < threshold) {
          sumX += x;
          sumY += y;
          count++;
        }
      }
    }
    // As long as we found something
    if (count != 0) {
      loc = new PVector(sumX/count,sumY/count);
    }

    // Interpolating the location, doing it arbitrarily for now
    lerpedLoc.x = PApplet.lerp(lerpedLoc.x, loc.x, 0.3f);
    lerpedLoc.y = PApplet.lerp(lerpedLoc.y, loc.y, 0.3f);
  }

  PVector getLerpedPos() {
    return lerpedLoc;
  }

  PVector getPos() {
    return loc;
  }

  void display() {
    PImage img = kinect.getDepthImage();

    // Being overly cautious here
    if (depth == null || img == null) return;

    // Going to rewrite the depth image to show which pixels are in threshold
    // A lot of this is redundant, but this is just for demonstration purposes
    display.loadPixels();
    for(int x = 0; x < kw; x++) {
      for(int y = 0; y < kh; y++) {
        // mirroring image
        int offset = kw-x-1+y*kw;
        // Raw depth
        int rawDepth = depth[offset];

        int pix = x+y*display.width;
        if (rawDepth < threshold) {
          // A red color instead
          display.pixels[pix] = color(245,100,100);
        } 
        else {
          display.pixels[pix] = img.pixels[offset];
        }
      }
    }
    display.updatePixels();

    // Draw the image
    image(display,0,0);
  }

  void quit() {
    kinect.quit();
  }

  int getThreshold() {
    return threshold;
  }

  void setThreshold(int t) {
    threshold =  t;
  }
}

Answer 1

There's two ways of doing this...

The way Daniel's code is accessing the coordinates right now is using a Two-Dimensional Vector (ie. with an X and Y). You could change it to a Three-Dimensional Vector (so it also stores a Z coordinate), and the OpenKinect library should return the Z coordinate in the same way it does the X and Y ... I think ;-) (have to check his source). But this will return you the Z coordinate of every pixel, which you will then have to loop over, which is cumbersome and computationally expensive...

Now, the way that Daniel is actually doing it in this example is finding the depth of a particular XY location and returning it to you if it is past a certain threshold... this is the rawDepth integer you see in the KinectTracker ... so it tests if this is less than the threshold value (which you can change) and if it does, it colours those pixels and writes them into an image buffer... and you can then ask for the XY coordinate of that image, for example, or pass it to a blob detection routine, et cetera ...

Answer 2

There are two main steps:

1. fetching the depths (which KinectTracker already does in the track() method)
2. getting the depth for the current pixel using an offset to find a position in a 1D depth array based on a 2D position (x,y) (which again is kind of done in the track() method: int offset = kw-x-1+y*kw;)

Note that the coordinate is mirrored though, usually to the index is computed like so:

index = y*width+x

as explained in the get() reference notes

So in theory all you need to this is something like this at the end of the track() method:

lerpedLoc.z = depth[kw-((int)lerpedLoc.x)-1+((int)lerpedLoc.y)*kw];

like so:

void track() {

    // Get the raw depth as array of integers
    depth = kinect.getRawDepth();

    // Being overly cautious here
    if (depth == null) return;

    float sumX = 0;
    float sumY = 0;
    float count = 0;

    for(int x = 0; x < kw; x++) {
      for(int y = 0; y < kh; y++) {
        // Mirroring the image
        int offset = kw-x-1+y*kw;
        // Grabbing the raw depth
        int rawDepth = depth[offset];

        // Testing against threshold
        if (rawDepth < threshold) {
          sumX += x;
          sumY += y;
          count++;
        }
      }
    }
    // As long as we found something
    if (count != 0) {
      loc = new PVector(sumX/count,sumY/count);
    }

    // Interpolating the location, doing it arbitrarily for now
    lerpedLoc.x = PApplet.lerp(lerpedLoc.x, loc.x, 0.3f);
    lerpedLoc.y = PApplet.lerp(lerpedLoc.y, loc.y, 0.3f);
    lerpedLoc.z = depth[kw-((int)lerpedLoc.x)-1+((int)lerpedLoc.y)*kw];
  }

I can't test with a kinect right now, but this should work. I'm not sure if you'll get the depth for the correct pixel or a mirrored one. The only other option is something like:

lerpedLoc.z = depth[((int)lerpedLoc.x)+((int)lerpedLoc.y)*kw];

Answer 3

Adding this at the end of void track() worked:

lerpedLoc.z = depth[kw-((int)lerpedLoc.x)-1+((int)lerpedLoc.y)*kw];

I then changed the last block in void draw() to this to read out the Z value:

// Display some info
int t = tracker.getThreshold();
fill(0);
text("Location-X: " + v1.x,10,500);
text("Location-Y: " + v1.y,10,530);
text("Location-Z: " + v2.z,10,560);  // <<Adding this worked!
text("threshold: " + t,10,590);