Kinect silhouette: body distortion

yuniwu

Hi everyone,

I've been trying to run this tutorial: http://www.creativeapplications.net/processing/kinect-physics-tutorial-for-processing/ My code can run, but the body detected is distorted. I think it is the problem that I add the enableDepth() function into the code. However, I cannot run the code if I delete this line. What can I do?

The scene I can get is like this:

My code:

    import blobDetection.*; // blobs
    import toxi.geom.*; // toxiclibs shapes and vectors
    import toxi.processing.*; // toxiclibs display
    import pbox2d.*; // shiffman's jbox2d helper library
    import org.jbox2d.collision.shapes.*; // jbox2d
    import org.jbox2d.common.*; // jbox2d
    import org.jbox2d.dynamics.*; // jbox2d
    import java.util.Collections.*; //added by Yuni 

    // declare SimpleOpenNI object
    SimpleOpenNI context;
    // declare BlobDetection object
    BlobDetection theBlobDetection;
    // ToxiclibsSupport for displaying polygons
    ToxiclibsSupport gfx;
    // declare custom PolygonBlob object (see class for more info)
    PolygonBlob poly;

    // PImage to hold incoming imagery and smaller one for blob detection
    PImage cam, blobs;
    // the kinect's dimensions to be used later on for calculations
    int kinectWidth = 640;
    int kinectHeight = 480;
    // to center and rescale from 640x480 to higher custom resolutions
    float reScale;

    // background and blob color
    color bgColor, blobColor;
    // three color palettes (artifact from me storing many interesting color palettes as strings in an external data file <span class="Emoticon Emoticon3"><span>;-)</span></span>
    String[] palettes = {
      "-1117720,-13683658,-8410437,-9998215,-1849945,-5517090,-4250587,-14178341,-5804972,-3498634", 
      "-67879,-9633503,-8858441,-144382,-4996094,-16604779,-588031", 
      "-1978728,-724510,-15131349,-13932461,-4741770,-9232823,-3195858,-8989771,-2850983,-10314372"
    };
    color[] colorPalette;

    // the main PBox2D object in which all the physics-based stuff is happening
    PBox2D box2d;
    // list to hold all the custom shapes (circles, polygons)
    ArrayList<CustomShape> polygons = new ArrayList<CustomShape>();

    void setup() {
      // it's possible to customize this, for example 1920x1080
      size(1280, 720, OPENGL);
      context = new SimpleOpenNI(this);
      // initialize SimpleOpenNI object
      //if (!context.enableUser()) { 
        // if context.enableScene() returns false
        // then the Kinect is not working correctly
        // make sure the green light is blinking
        //println("Kinect not connected!"); 
        //exit();
      //} else {
        // mirror the image to be more intuitive
        context.setMirror(true);
        context.enableDepth();
        context.enableUser();
        // calculate the reScale value
        // currently it's rescaled to fill the complete width (cuts of top-bottom)
        // it's also possible to fill the complete height (leaves empty sides)
        reScale = (float) width / kinectWidth;
        // create a smaller blob image for speed and efficiency
        blobs = createImage(kinectWidth/3, kinectHeight/3, RGB);
        // initialize blob detection object to the blob image dimensions
        theBlobDetection = new BlobDetection(blobs.width, blobs.height);
        theBlobDetection.setThreshold(0.6);
        // initialize ToxiclibsSupport object
        gfx = new ToxiclibsSupport(this);
        // setup box2d, create world, set gravity
        box2d = new PBox2D(this);
        box2d.createWorld();
        box2d.setGravity(0, -20);
        // set random colors (background, blob)
        setRandomColors(1);
      }
    //}

    void draw() {
      background(bgColor);
      // update the SimpleOpenNI object
      context.update();
      // put the image into a PImage
      cam = context.userImage().get();
      // copy the image into the smaller blob image
      blobs.copy(cam, 0, 0, cam.width, cam.height, 0, 0, blobs.width, blobs.height);
      // blur the blob image
      blobs.filter(BLUR, 1);
      // detect the blobs
      theBlobDetection.computeBlobs(blobs.pixels);
      // initialize a new polygon
      poly = new PolygonBlob();
      // create the polygon from the blobs (custom functionality, see class)
      poly.createPolygon();
      // create the box2d body from the polygon
      poly.createBody();
      // update and draw everything (see method)
      updateAndDrawBox2D();
      // destroy the person's body (important!)
      poly.destroyBody();
      // set the colors randomly every 240th frame
      setRandomColors(240);
    }

    void updateAndDrawBox2D() {
      // if frameRate is sufficient, add a polygon and a circle with a random radius
      if (frameRate > 29) {
        polygons.add(new CustomShape(kinectWidth/2, -50, -1));
        polygons.add(new CustomShape(kinectWidth/2, -50, random(2.5, 20)));
      }
      // take one step in the box2d physics world
      box2d.step();

      // center and reScale from Kinect to custom dimensions
      translate(0, (height-kinectHeight*reScale)/2);
      scale(reScale);

      // display the person's polygon  
      noStroke();
      fill(blobColor);
      gfx.polygon2D(poly);

      // display all the shapes (circles, polygons)
      // go backwards to allow removal of shapes
      for (int i=polygons.size()-1; i>=0; i--) {
        CustomShape cs = polygons.get(i);
        // if the shape is off-screen remove it (see class for more info)
        if (cs.done()) {
          polygons.remove(i);
        // otherwise update (keep shape outside person) and display (circle or polygon)
        } else {
          cs.update();
          cs.display();
        }
      }
    }

    // sets the colors every nth frame
    void setRandomColors(int nthFrame) {
      if (frameCount % nthFrame == 0) {
        // turn a palette into a series of strings
        String[] paletteStrings = split(palettes[int(random(palettes.length))], ",");
        // turn strings into colors
        colorPalette = new color[paletteStrings.length];
        for (int i=0; i<paletteStrings.length; i++) {
          colorPalette[i] = int(paletteStrings[i]);
        }
        // set background color to first color from palette
        bgColor = colorPalette[0];
        // set blob color to second color from palette
        blobColor = colorPalette[1];
        // set all shape colors randomly
        for (CustomShape cs: polygons) { cs.col = getRandomColor(); }
      }
    }

    // returns a random color from the palette (excluding first aka background color)
    color getRandomColor() {
      return colorPalette[int(random(1, colorPalette.length))];
    }// usually one would probably make a generic Shape class and subclass different types (circle, polygon), but that
// would mean at least 3 instead of 1 class, so for this tutorial it's a combi-class CustomShape for all types of shapes
// to save some space and keep the code as concise as possible I took a few shortcuts to prevent repeating the same code
class CustomShape {
  // to hold the box2d body
  Body body;
  // to hold the Toxiclibs polygon shape
  Polygon2D toxiPoly;
  // custom color for each shape
  color col;
  // radius (also used to distinguish between circles and polygons in this combi-class
  float r;

  CustomShape(float x, float y, float r) {
    this.r = r;
    // create a body (polygon or circle based on the r)
    makeBody(x, y);
    // get a random color
    col = getRandomColor();
  }

  void makeBody(float x, float y) {
    // define a dynamic body positioned at xy in box2d world coordinates,
    // create it and set the initial values for this box2d body's speed and angle
    BodyDef bd = new BodyDef();
    bd.type = BodyType.DYNAMIC;
    bd.position.set(box2d.coordPixelsToWorld(new Vec2(x, y)));
    body = box2d.createBody(bd);
    body.setLinearVelocity(new Vec2(random(-8, 8), random(2, 8)));
    body.setAngularVelocity(random(-5, 5));

    // depending on the r this combi-code creates either a box2d polygon or a circle
    if (r == -1) {
      // box2d polygon shape
      PolygonShape sd = new PolygonShape();
      // toxiclibs polygon creator (triangle, square, etc)
      toxiPoly = new Circle(random(5, 20)).toPolygon2D(int(random(3, 6)));
      // place the toxiclibs polygon's vertices into a vec2d array
      Vec2[] vertices = new Vec2[toxiPoly.getNumPoints()];
      for (int i=0; i<vertices.length; i++) {
        Vec2D v = toxiPoly.vertices.get(i);
        vertices[i] = box2d.vectorPixelsToWorld(new Vec2(v.x, v.y));
      }
      // put the vertices into the box2d shape
      sd.set(vertices, vertices.length);
      // create the fixture from the shape (deflect things based on the actual polygon shape)
      body.createFixture(sd, 1);
    } else {
      // box2d circle shape of radius r
      CircleShape cs = new CircleShape();
      cs.m_radius = box2d.scalarPixelsToWorld(r);
      // tweak the circle's fixture def a little bit
      FixtureDef fd = new FixtureDef();
      fd.shape = cs;
      fd.density = 1;
      fd.friction = 0.01;
      fd.restitution = 0.3;
      // create the fixture from the shape's fixture def (deflect things based on the actual circle shape)
      body.createFixture(fd);
    }
  }

  // method to loosely move shapes outside a person's polygon
  // (alternatively you could allow or remove shapes inside a person's polygon)
  void update() {
    // get the screen position from this shape (circle of polygon)
    Vec2 posScreen = box2d.getBodyPixelCoord(body);
    // turn it into a toxiclibs Vec2D
    Vec2D toxiScreen = new Vec2D(posScreen.x, posScreen.y);
    // check if this shape's position is inside the person's polygon
    boolean inBody = poly.containsPoint(toxiScreen);
    // if a shape is inside the person
    if (inBody) {
      // find the closest point on the polygon to the current position
      Vec2D closestPoint = toxiScreen;
      float closestDistance = 9999999;
      for (Vec2D v : poly.vertices) {
        float distance = v.distanceTo(toxiScreen);
        if (distance < closestDistance) {
          closestDistance = distance;
          closestPoint = v;
        }
      }
      // create a box2d position from the closest point on the polygon
      Vec2 contourPos = new Vec2(closestPoint.x, closestPoint.y);
      Vec2 posWorld = box2d.coordPixelsToWorld(contourPos);
      float angle = body.getAngle();
      // set the box2d body's position of this CustomShape to the new position (use the current angle)
      body.setTransform(posWorld, angle);
    }
  }

  // display the customShape
  void display() {
    // get the pixel coordinates of the body
    Vec2 pos = box2d.getBodyPixelCoord(body);
    pushMatrix();
    // translate to the position
    translate(pos.x, pos.y);
    noStroke();
    // use the shape's custom color
    fill(col);
    // depending on the r this combi-code displays either a polygon or a circle
    if (r == -1) {
      // rotate by the body's angle
      float a = body.getAngle();
      rotate(-a); // minus!
      gfx.polygon2D(toxiPoly);
    } else {
      ellipse(0, 0, r*2, r*2);
    }
    popMatrix();
  }

  // if the shape moves off-screen, destroy the box2d body (important!)
  // and return true (which will lead to the removal of this CustomShape object)
  boolean done() {
    Vec2 posScreen = box2d.getBodyPixelCoord(body);
    boolean offscreen = posScreen.y > height;
    if (offscreen) {
      box2d.destroyBody(body);
      return true;
    }
    return false;
  }
}
// an extended polygon class quite similar to the earlier PolygonBlob class (but extending Toxiclibs' Polygon2D class instead)
// The main difference is that this one is able to create (and destroy) a box2d body from it's own shape
class PolygonBlob extends Polygon2D {
  // to hold the box2d body
  Body body;

  // the createPolygon() method is nearly identical to the one presented earlier
  // see the Kinect Flow Example for a more detailed description of this method (again, feel free to improve it)
  void createPolygon() {
    ArrayList<ArrayList<PVector>> contours = new ArrayList<ArrayList<PVector>>();
    int selectedContour = 0;
    int selectedPoint = 0;

    // create contours from blobs
    for (int n=0 ; n<theBlobDetection.getBlobNb(); n++) {
      Blob b = theBlobDetection.getBlob(n);
      if (b != null && b.getEdgeNb() > 100) {
        ArrayList<PVector> contour = new ArrayList<PVector>();
        for (int m=0; m<b.getEdgeNb(); m++) {
          EdgeVertex eA = b.getEdgeVertexA(m);
          EdgeVertex eB = b.getEdgeVertexB(m);
          if (eA != null && eB != null) {
            EdgeVertex fn = b.getEdgeVertexA((m+1) % b.getEdgeNb());
            EdgeVertex fp = b.getEdgeVertexA((max(0, m-1)));
            float dn = dist(eA.x*kinectWidth, eA.y*kinectHeight, fn.x*kinectWidth, fn.y*kinectHeight);
            float dp = dist(eA.x*kinectWidth, eA.y*kinectHeight, fp.x*kinectWidth, fp.y*kinectHeight);
            if (dn > 15 || dp > 15) {
              if (contour.size() > 0) {
                contour.add(new PVector(eB.x*kinectWidth, eB.y*kinectHeight));
                contours.add(contour);
                contour = new ArrayList<PVector>();
              } else {
                contour.add(new PVector(eA.x*kinectWidth, eA.y*kinectHeight));
              }
            } else {
              contour.add(new PVector(eA.x*kinectWidth, eA.y*kinectHeight));
            }
          }
        }
      }
    }

    while (contours.size() > 0) {

      // find next contour
      float distance = 999999999;
      if (getNumPoints() > 0) {
        Vec2D vecLastPoint = vertices.get(getNumPoints()-1);
        PVector lastPoint = new PVector(vecLastPoint.x, vecLastPoint.y);
        for (int i=0; i<contours.size(); i++) {
          ArrayList<PVector> c = contours.get(i);
          PVector fp = c.get(0);
          PVector lp = c.get(c.size()-1);
          if (fp.dist(lastPoint) < distance) { 
            distance = fp.dist(lastPoint); 
            selectedContour = i; 
            selectedPoint = 0;
          }
          if (lp.dist(lastPoint) < distance) { 
            distance = lp.dist(lastPoint); 
            selectedContour = i; 
            selectedPoint = 1;
          }
        }
      } else {
        PVector closestPoint = new PVector(width, height);
        for (int i=0; i<contours.size(); i++) {
          ArrayList<PVector> c = contours.get(i);
          PVector fp = c.get(0);
          PVector lp = c.get(c.size()-1);
          if (fp.y > kinectHeight-5 && fp.x < closestPoint.x) { 
            closestPoint = fp; 
            selectedContour = i; 
            selectedPoint = 0;
          }
          if (lp.y > kinectHeight-5 && lp.x < closestPoint.y) { 
            closestPoint = lp; 
            selectedContour = i; 
            selectedPoint = 1;
          }
        }
      }

      // add contour to polygon
      ArrayList<PVector> contour = contours.get(selectedContour);
      if (selectedPoint > 0) { java.util.Collections.reverse(contour); }
      for (PVector p : contour) {
        add(new Vec2D(p.x, p.y));
      }
      contours.remove(selectedContour);
    }
  }

  // creates a shape-deflecting physics chain in the box2d world from this polygon
  void createBody() {
    // for stability the body is always created (and later destroyed)
    BodyDef bd = new BodyDef();
    body = box2d.createBody(bd);
    // if there are more than 0 points (aka a person on screen)...
    if (getNumPoints() > 0) {
      // create a vec2d array of vertices in box2d world coordinates from this polygon
      Vec2[] verts = new Vec2[getNumPoints()];
      for (int i=0; i<getNumPoints(); i++) {
        Vec2D v = vertices.get(i);
        verts[i] = box2d.coordPixelsToWorld(v.x, v.y);
      }
      // create a chain from the array of vertices
      ChainShape chain = new ChainShape();
      chain.createChain(verts, verts.length);
      // create fixture in body from the chain (this makes it actually deflect other shapes)
      body.createFixture(chain, 1);
    }
  }

  // destroy the box2d body (important!)
  void destroyBody() {
    box2d.destroyBody(body);
  }
}

velvetkevorkian

If you run other Kinect examples, do you get similar distortion? How far from the Kinect are you? If you're too close or too far the depth camera won't work correctly, which would be my guess as to what's happening here. It may depend on your environment, e.g. are there lights or windows that may be interfering with the Kinect's IR sensor.

yuniwu

I didn't get this distortion when I run another background subtraction example. So I think it is not the problem of the environment:(

Pablovitamin

Hello I am trying to run your code in order to try to solve the problem but I am having problems with the Pbox2D library.

I can only find the shiffman.box2d.*;

Thanks.

yuniwu

Thanks Pablovitamin,

The Pbox2D library is downloaded in this link: http://www.shiffman.net/p5/libraries/box2d_processing/box2d_processing.zip

Please help me:(

yuniwu

Can anyone help me please:( Please. I really need it, very urgent:(

modqhx

@yuniwu- Hey, were you able to solve the problem? I am having the same problem! :( Since I got the code from amnon recent post on the forum.. Maybe @amnon can help us out? Or maybe some problem with the blob detection version? Did you try comparing with this? : http://forum.processing.org/two/discussion/2625/can-kinect-physics-code-examples#Item_7