KinectPV2 library Color image to depth issue

jackthegoodboy

Hey!

I'm trying to map color pixels to the depth map using the KinectPV2 library, in order to do some checkerboard detection for calibration.

I've been using the function getMapDepthToColor() which gives you a mapping between the 2 spaces (or so I think).

The following is a modification of the MapDepthToColor example to suit my needs.

import KinectPV2.*;

KinectPV2 kinect;

int [] depthZero;

//BUFFER ARRAY TO CLEAN DE PIXLES
PImage depthToColorImg;

void setup() {
  size(1024, 848, P3D);

  depthToColorImg = createImage(512, 424, PImage.RGB);
  depthZero    = new int[ KinectPV2.WIDTHDepth * KinectPV2.HEIGHTDepth];

  //SET THE ARRAY TO 0s
  for (int i = 0; i < KinectPV2.WIDTHDepth; i++) {
    for (int j = 0; j < KinectPV2.HEIGHTDepth; j++) {
      depthZero[424*i + j] = 0;
    }
  }

  kinect = new KinectPV2(this);
  kinect.enableDepthImg(true);
  kinect.enableColorImg(true);
  kinect.enablePointCloud(true);

  kinect.init();
}

void draw() {
  background(0);

  float [] mapDCT = kinect.getMapDepthToColor();

  //get the raw data from depth and color
  int [] colorRaw = kinect.getRawColor();
  int [] depthRaw = kinect.getRawDepthData();

  //clean de pixels
  PApplet.arrayCopy(depthZero, depthToColorImg.pixels);

  int count = 0;
  depthToColorImg.loadPixels();
  count = 0;

  for (int i = 0; i < KinectPV2.HEIGHTDepth; i++) {
    for (int j = 0; j < KinectPV2.WIDTHDepth; j++) {

      //incoming pixels 512 x 424 with position in 1920 x 1080
      int valX = (int)mapDCT[(count) * 2 + 0];
      int valY = (int)mapDCT[(count) * 2 + 1];

      if (valX >= 0 && valX < 1920 && valY >= 0 && valY < 1080) {
        color colorPixel = colorRaw[valY * 1920 + valX];
        depthToColorImg.pixels[count] = colorPixel;
      }
      count++;
    }
  }

  depthToColorImg.updatePixels();

  image(depthToColorImg, 0, 424);
  image(kinect.getColorImage(), 0, 0, 512, 424);
  image(kinect.getDepthImage(), 512, 0);
}

Giving the output:

Is there something I'm misunderstanding here? Any help would be much appreciated.