kinect depth data via UDP

dwbowen

Hello all,

I am attempting to broadcast kinect depth data from one computer to another via UDP packets but I am not having any success. I have adapted the Shiffman Point Cloud example to skip every 4th point of depth data. I am then converting the int[] array of skipped data into a byte[] array. Then I try to send it in a UDP packet using "ds.send(new DatagramPacket()". Here is my code:

//peasy library for v1.5
import peasy.*;
PeasyCam cam;

//hypermediaUDP
//import hypermedia.net.*;
//UDP udp;  // define the UDP object

import javax.imageio.*;
import java.awt.image.*; 
import java.net.*;
import java.io.*; 
import java.nio.*;

//java.netUDP
// This is the port we are sending to
int clientPort = 9100; 
// This is our object that sends UDP out
DatagramSocket ds;

//depthZ stuff
int depthZ;
int array_count;
int[] depthZ_int_array;
byte[] depthZ_byte_array;

// skip factor
int skip = 4;

// Kinect Library object
import org.openkinect.*;
import org.openkinect.processing.*;
Kinect kinect;

// Size of kinect image
int w = 480; //640*
int h = 480; //480*

// We'll use a lookup table so that we don't have to repeat the math over and over
float[] depthLookUp = new float[2048];

void setup() {
  size(1440, 810, P3D);
  kinect = new Kinect(this);
  kinect.start();
  kinect.enableDepth(true);
  // We don't need the grayscale image in this example
  // so this makes it more efficient
  kinect.processDepthImage(false);

  cam = new PeasyCam(this, 1000);
  cam.setMinimumDistance(50);
  cam.setMaximumDistance(1000);

  // Lookup table for all possible depth values (0 - 2047)
  for (int i = 0; i < depthLookUp.length; i++) {
    depthLookUp[i] = rawDepthToMeters(i);
  }

  //define array_count based on skip  
  array_count = 0;

  for (int x=0; x<w; x+=skip) {
    for (int y=0; y<h; y+=skip) {
      array_count++;
    }
  }  

  //debug
  //println(array_count);

  depthZ_int_array = new int[array_count];
  depthZ_byte_array = new byte[array_count*4];

  array_count = 0; 

  //hypermediaUPD
  //udp = new UDP( this, 6000 );  // create a new datagram connection on port 6000
}

void draw() {

  background(0);
  fill(255);

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

  translate(0, 0, 20);

  rotateX(0);
  rotateY(0);
  rotateZ(0);
  pushMatrix();
  popMatrix();

  for (int x=0; x<w; x+=skip) {
    for (int y=0; y<h; y+=skip) {
      int offset = x+y*w;

      // Convert kinect data to world xyz coordinate
      int rawDepth = depth[offset]; 
      PVector v = depthToWorld(x, y, rawDepth);

      stroke(255);
      pushMatrix();
      // Scale up by 200
      float factor = 200;
      translate(v.x*factor, v.y*factor, factor-v.z*factor);
      // Draw a point
      point(0, 0);
      popMatrix();

      depthZ_int_array[array_count] = (rawDepth); 

      //array_count = array_count + 1;
      array_count++;
    }
  }

  //convert int array to byte array
  ByteBuffer byteBuffer = ByteBuffer.allocate(depthZ_int_array.length * 4);        
  IntBuffer intBuffer = byteBuffer.asIntBuffer();
  intBuffer.put(depthZ_int_array);

  depthZ_byte_array = byteBuffer.array();

  //print array lengths
  //println(depthZ_int_array.length + " " + depthZ_byte_array.length);

  try {
    //print UDP info
    println("Sending datagram with " + depthZ_byte_array.length + " bytes");
    ds.send(new DatagramPacket(depthZ_byte_array, depthZ_byte_array.length, InetAddress.getByName("localhost"), clientPort));
  } 
  catch (Exception e) {
    e.printStackTrace();
  }

  array_count = 0;
}

// These functions come from: http://graphics.stanford.edu/~mdfisher/Kinect.html
float rawDepthToMeters(int depthValue) {
  if (depthValue < 2047) {
    return (float)(1.0 / ((double)(depthValue) * -0.0030711016 + 3.3309495161));
  }
  return 0.0f;
}

PVector depthToWorld(int x, int y, int depthValue) {

  final double fx_d = 1.0 / 5.9421434211923247e+02;
  final double fy_d = 1.0 / 5.9104053696870778e+02;
  final double cx_d = 3.3930780975300314e+02;
  final double cy_d = 2.4273913761751615e+02;

  PVector result = new PVector();
  double depth =  depthLookUp[depthValue];//rawDepthToMeters(depthValue);
  result.x = (float)((x - cx_d) * depth * fx_d);
  result.y = (float)((y - cy_d) * depth * fy_d);
  result.z = (float)(depth);
  return result;
}

In processing v1.5 I receive the following errors:

java.lang.NullPointerException
    at kinect_UDP_skip4.draw(kinect_UDP_skip4.java:167)
    at processing.core.PApplet.handleDraw(PApplet.java:1631)
    at processing.core.PApplet.run(PApplet.java:1530)
    at java.lang.Thread.run(Thread.java:695)

And in processing v2.2.1 I get these errors:

java.lang.NullPointerException
    at kinect_datareader_4pointcloud_UDP.draw(kinect_datareader_4pointcloud_UDP.java:132)
    at processing.core.PApplet.handleDraw(PApplet.java:2386)
    at processing.opengl.PJOGL$PGLListener.display(PJOGL.java:862)
    at jogamp.opengl.GLDrawableHelper.displayImpl(GLDrawableHelper.java:665)
    at jogamp.opengl.GLDrawableHelper.display(GLDrawableHelper.java:649)
    at javax.media.opengl.awt.GLCanvas$10.run(GLCanvas.java:1289)
    at jogamp.opengl.GLDrawableHelper.invokeGLImpl(GLDrawableHelper.java:1119)
    at jogamp.opengl.GLDrawableHelper.invokeGL(GLDrawableHelper.java:994)
    at javax.media.opengl.awt.GLCanvas$11.run(GLCanvas.java:1300)
    at java.awt.event.InvocationEvent.dispatch(InvocationEvent.java:302)
    at java.awt.EventQueue.dispatchEventImpl(EventQueue.java:733)
    at java.awt.EventQueue.access$200(EventQueue.java:103)
    at java.awt.EventQueue$3.run(EventQueue.java:694)
    at java.awt.EventQueue$3.run(EventQueue.java:692)
    at java.security.AccessController.doPrivileged(Native Method)
    at java.security.ProtectionDomain$1.doIntersectionPrivilege(ProtectionDomain.java:76)
    at java.awt.EventQueue.dispatchEvent(EventQueue.java:703)
    at java.awt.EventDispatchThread.pumpOneEventForFilters(EventDispatchThread.java:242)
    at java.awt.EventDispatchThread.pumpEventsForFilter(EventDispatchThread.java:161)
    at java.awt.EventDispatchThread.pumpEventsForHierarchy(EventDispatchThread.java:150)
    at java.awt.EventDispatchThread.pumpEvents(EventDispatchThread.java:146)
    at java.awt.EventDispatchThread.pumpEvents(EventDispatchThread.java:138)
    at java.awt.EventDispatchThread.run(EventDispatchThread.java:91)

Any ideas of what is wrong or a better way to do this?

thank you,

dwbowen

Turns out I did not setup my DatagramSocket....

  // Setting up the DatagramSocket, requires try/catch
  try {
    ds = new DatagramSocket();
  } 
  catch (SocketException e) {
    e.printStackTrace();
  }

thanks,