Tracking a moving object and calculating/drawing its trajectory in space

edited February 2017 in Kinect

I am trying to write a sketch that does the following: the users clicks on an object on the videocamera image, and then the computer tracks this object from its colour and finds its distance from the Kinect. By storing all the distance measurements in an array, the trajectory of that moving object can be calculated, using 3d polar (spherical) coordinate system, and then be drawn on the screen. In summary, the tracking of the object is done by Kinect’s videocamera alone , and the distance is found by the depth sensor. Then the x,y,z coordinates are calculated from the spherical coordinate system.

The tracking in my code works (sort of, it's far from perfect but it's good enough for now), my problem is when I try to store the values of the coordinates of the object (the two coordinates on the video and the distance) in three different arrayLists, from which I then calculate the spherical coordinates, and then its x,y,z. Here's my sketch, the problem appears inside the Trajectory class. Any ideas about what might be wrong with my ArrayLists?

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


Kinect kinect;

float deg;

boolean colorDepth = false;

color trackColor;

float threshold=5;
int count=0;

PImage videoImg;
PImage depthImg;

float aX=0;
float aY=0;

ArrayList apos = new ArrayList();
ArrayList bpos = new ArrayList();
ArrayList cpos = new ArrayList();

Trajectory trajectory;


void setup() {
  size(1280, 480);

  kinect = new Kinect(this);
  kinect.initDepth();
  kinect.initVideo();

  kinect.enableColorDepth(colorDepth);

  deg = kinect.getTilt();

  trackColor = color(0, 0, 255);
}





void draw() {

  //DEPTH IMAGE
  depthImg=kinect.getDepthImage();
  image (depthImg, 640, 0);

  //VIDEO IMAGE
  videoImg=kinect.getVideoImage();

  videoImg.loadPixels();

  int count=0;

  float avgX=0;
  float avgY=0;

  for (int x=0; x<videoImg.width; x++) {
    for (int y=0; y<videoImg.height; y++) {
      int loc = x+ y*videoImg.width; 

      color currentColor=videoImg.pixels[loc];

      float r1=red(currentColor);
      float g1=green(currentColor);
      float b1=blue(currentColor);
      float r2=red(trackColor);
      float g2=green(trackColor);
      float b2=blue(trackColor);

      float d=dist(r1, g1, b1, r2, g2, b2);

      if (d<threshold) {
        avgX +=x;
        avgY+= y;

        stroke(255);
        strokeWeight (2);
        point(x, y);

        count ++;
      }
    }
  }
  videoImg.updatePixels();
  image (videoImg, 0, 0);

  if (count > 0) { 

    aX=avgX/count;
    aY=avgY/count;

    fill(trackColor);
    strokeWeight(4.0);
    stroke(255);
    ellipse(aX, aY, 32, 32);
    ellipse(aX+640, aY, 32, 32);

    //println (count);
  }
  fill(0);
  text("threshold=" + threshold, 40, 30);
  text("trackColor=" + trackColor, 40, 60);



  //DEPTH 


  int[] depth =kinect.getRawDepth();
  int locDepth= int(aX)+ int(aY)*videoImg.width; 

  float myDist=depth[locDepth];
  float myDistConverted=rawDepthToMeters(int(myDist));

  fill(255,0,0);
  textSize(18);
  text( "Distance=" + myDistConverted + " meters " , 680, 30);


  //Trajectory

  apos.add(aX);
  bpos.add(aY);
  cpos.add(myDist);

  trajectory=new Trajectory (apos, bpos, cpos);
  trajectory.display();

  if (apos.size()>500){
    apos.remove(0);
    bpos.remove(0);
    cpos.remove(0);
  }


}

void mousePressed() {
  // Save color where the mouse is clicked in trackColor variable
  int loc = mouseX + mouseY*videoImg.width;
  trackColor = videoImg.pixels[loc];
}

void keyPressed() {
  if (key == CODED) {
    if (keyCode == UP) {
      deg++;
    } else if (keyCode == DOWN) {
      deg--;
    }
    deg = constrain(deg, 0, 30);
    kinect.setTilt(deg);
  }
}

//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;
}








class Trajectory {

  //Polar coordinates

  float r;
  float t; //first angle of polar coordinates;
  float f; //second angle of polar coordinates;

  ArrayList rDist = new ArrayList();
  ArrayList tAngle = new ArrayList();
  ArrayList fAngle = new ArrayList();

  //x,y,z coordinates

  ArrayList xArray = new ArrayList();
  ArrayList yArray = new ArrayList();
  ArrayList zArray = new ArrayList();

  float x;
  float y;
  float z;



  Trajectory(ArrayList aposition, ArrayList bposition, ArrayList cposition) {

    //CALCULATE SPHERICAL COORDINATES d,t,f


    for (int i=1; i<cposition.size(); i++) { 

      r = cposition.get(i);
      rDist.add(r);
    }

    for (int i=1; i<bposition.size(); i++) { 
      //sin(t)=z/d;
      t= asin(bposition.get(i)/rDist.get(i));
      tAngle.add(t);
    }

    for (int i=1; i<aposition.size(); i++) { 
      //sin(f)=x/(d*cos(t));
      f=asin(aposition.get(i)/(rDist.get(i)*cos(tAngle.get(i))));
      fAngle.add(f);
    }


    //CALCULATE COORDINATES x,y,z
for (int i=1; i<rDist.size(); i++) { 
    x=rDist.get(i)*sin(tAngle.get(i))*cos(fAngle.get(i));
    xArray.add(x);

    y=r(i)*sin(tAngle.get(i))*sin(fAngle.get(i));
    yArray.add(y);

    z=rDist(i)*cos(tAngle(i));
    zArray.add(z);
  }
}


void display() {  
  for (int i=1; i<xArray.size(); i++) {
    stroke(255);
    strokeWeight(10);
    line (xArray.get(i-1), yArray.get(i-1), zArray.get(i-1), xArray.get(i), yArray.get(i), zArray.get(i));
  }
}
}
Sign In or Register to comment.