Mapping rgbMap to depthMap

Question

So I'm trying to map the pixel on the rgbMap to one on the depthMap so I can get the depth from a color tracked object.

I've noticed the depth map is smaller than the rgb map in addition to the different POV from being on a different spot on the kinect itself.

Is there an efficient way to do this that I'm unaware of? Currently I'm thinking of trying to line up the example pixels and then try to figure out an equation for the offset as you get farther away.

Code below, please not that the color tracking is far from done, and that I'm using mouse pressed for testing the pixel position.

    /**
     * ControlP5 Controlframe
     * with controlP5 2.0 all java.awt dependencies have been removed
     * as a consequence the option to display controllers in a separate
     * window had to be removed as well. 
     * this example shows you how to create a java.awt.frame and use controlP5
     *
     * by Andreas Schlegel, 2012
     * www.sojamo.de/libraries/controlp5
     *
     */

    import java.awt.Frame;
    import java.awt.BorderLayout;
    import controlP5.*;
    import SimpleOpenNI.*;

    private ControlP5 cp5;



    ControlFrame cf;
    SimpleOpenNI  context;
    Ktracker p,o,b;
    float rx,ry,dx,dy;
    int def;


    void setup() {
      size((2 * 640),480);
      cp5 = new ControlP5(this);
      o = new Ktracker(188,57,49);
      // by calling function addControlFrame() a
      // new frame is created and an instance of class
      // ControlFrame is instanziated.
      cf = addControlFrame("Output", 640,480);
      // add Controllers to the 'extra' Frame inside 
      // the ControlFrame class setup() method below.
      context = new SimpleOpenNI(this);
      context.enableRGB();
      context.enableDepth();

      smooth();
      rx=context.rgbWidth()/2;
      ry=height/2;
    }

    void draw() {
      context.update(); 
      background(0,150,0);
      image(context.depthImage(),context.rgbWidth(),0);
      image(context.rgbImage(), 0,0); 
      int[] dm = context.depthMap();
      o.run();

      rectMode(CENTER);
      noStroke();
      fill(255,255,0);
      rect(rx,ry,5,5);
      dx=rx;
      dy=ry;
      fill(0,255,255);
      rect(dx+context.rgbWidth(),dy,5,5);
      rectMode(CORNER);

      if(o.wr<10){

        fill(0,0,255);
        noStroke();
        ellipse(o.currentPoints[0]+context.rgbWidth(),o.currentPoints[1],10,10);

        cf.z = dm[int(o.currentPoints[0]*o.currentPoints[1])];
      } else {
        cf.z=0;
      }

    }

    void mousePressed(){
      rx=mouseX;
      ry=mouseY;
    }


    ControlFrame addControlFrame(String theName, int theWidth, int theHeight) {
      Frame f = new Frame(theName);
      ControlFrame p = new ControlFrame(this, theWidth, theHeight);
      f.add(p);
      p.init();
      f.setTitle(theName);
      f.setSize(p.w, p.h);
      f.setLocation(100, 100);
      f.setResizable(false);
      f.setVisible(true);
      return p;
    }

    // the ControlFrame class extends PApplet, so we 
    // are creating a new processing applet inside a
    // new frame with a controlP5 object loaded
    public class ControlFrame extends PApplet {

      int w, h;
      float z;
      int abc = 100;

      public void setup() {
        size(w, h, P3D);
        frameRate(25);
        z=0;
      }

      public void draw() {
          background(abc);
          translate(width/2,height/2,z/100);
          sphere(250);

      }

      private ControlFrame() {
      }

      public ControlFrame(Object theParent, int theWidth, int theHeight) {
        parent = theParent;
        w = theWidth;
        h = theHeight;
      }

      public ControlP5 control() {
        return cp5;
      }


      ControlP5 cp5;

      Object parent;


    }
    class Ktracker{
     float mx,my,wr;
    // A variable for the color we are searching for.
     color trackColor;
     float[] currentPoints, prevPoints;

     Ktracker(int r,int g, int b){
      trackColor = color(r,g,b);

      currentPoints = new float[2];
      prevPoints = new float[2];

      prevPoints[0]=0;
      prevPoints[1]=0;
     }


     void run(){
      loadPixels();

      // Before we begin searching, the "world record" for closest color is set to a high number that is easy for the first pixel to beat.
      float worldRecord = 500; 

      // XY coordinate of closest color
      int closestX = 0;
      int closestY = 0;

      // Begin loop to walk through every pixel
      for (int x = 0; x < width/2; x ++ ) {
        for (int y = 0; y < height; y ++ ) {
          int loc = x + y*width;
          // What is current color
          color currentColor = 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);

          // Using euclidean distance to compare colors
          float d = dist(r1,g1,b1,r2,g2,b2); // We are using the dist( ) function to compare the current color with the color we are tracking.

          // If current color is more similar to tracked color than
          // closest color, save current location and current difference
          if (d < worldRecord) {
            worldRecord = d;
            closestX = x;
            closestY = y;
            currentPoints[0] = closestX;
            currentPoints[1] = closestY;
          }
        }
      }
      wr=worldRecord;
      // We only consider the color found if its color distance is less than 10. 
      // This threshold of 10 is arbitrary and you can adjust this number depending on how accurate you require the tracking to be.
      if (worldRecord < 10) { 

        // Draw a circle at the tracked pixel
        fill(255,0,0);
        //strokeWeight(4.0);
        stroke(0,0);
        ellipse(currentPoints[0],currentPoints[1],16,16);

      }
      println("wr: "+wr);
      println("worldRecord: "+worldRecord);
     }


    }

Answer 1

If you want to align the depth map with the RGB map you need to simply tell OpenNI to do so fo you in setup:

context.alternativeViewPointDepthToImage();

Have a look at the AlternativeViewpoint3d sketch in Examples > SimpleOpenNI > OpenNI

OpenNI has the registration done already so all you need to do is enable it when you need it.