ProcessingでKinect - Depth画像とCamera画像から3D空間を再現する

Depth画像とCamera画像から3D空間を再現できたら面白いんじゃないかと思ってやってみました。

というか、simple-openniのサンプルにあったののほぼパクリです…^^;

ソースコード

import processing.core.*;
import SimpleOpenNI.*;

@SuppressWarnings("serial")
public class AltViewpoint3dPlayer extends PApplet {
	private SimpleOpenNI context;
	private float zoomF = 0.2f;
	private float rotX = radians(180); // by default rotate the hole scene 180deg around the x-axis,
	                                   // the data from openni comes upside down
	private float rotY = radians(0);

	public void setup() {
		size(640, 480, P3D);

		context = new SimpleOpenNI(this);

		// disable mirror
		context.setMirror(false);

		// enable depthMap generation 
		if(context.enableDepth() == false) {
			println("Can't open the depthMap !!!"); 
			exit();
		}

		if(context.enableRGB() == false) {
			println("Can't open the rgbMap !!!"); 
			exit();
		}
	  
		// align depth data to image data
		context.alternativeViewPointDepthToImage();

		stroke(255,255,255);
		smooth();
		perspective(radians(45), (float)width / (float)height, 10, 150000);
	}

	public void draw() {
		context.update();

		background(0,0,0);

		translate(width/2, height/2, 0);
		rotateX(rotX);
		rotateY(rotY);
		scale(zoomF);

		PImage rgbImage = context.rgbImage();
		int[] depthMap = context.depthMap();
		int steps = 2; // to speed up the drawing, draw every third point
		int index;
		PVector realWorldPoint;
		int pixelColor;
	 
		strokeWeight(steps);

		translate(0, 0, -1000);  // set the rotation center of the scene 1000 infront of the camera

		PVector[] realWorldMap = context.depthMapRealWorld();

		for(int y = 0; y < context.depthHeight(); y+=steps) {
			for(int x=0;x < context.depthWidth();x+=steps) {
				index = x + y * context.depthWidth();
				if(depthMap[index] > 0) { 
					// get the color of the point
					pixelColor = rgbImage.pixels[index];
					stroke(pixelColor);
	        
					// draw the projected point
					realWorldPoint = realWorldMap[index];
					point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z); // make realworld z negative, in the 3d drawing coordsystem +z points in the direction of the eye
				}
			}
		} 

		// draw the kinect cam
		context.drawCamFrustum();
	}


	public void keyPressed() {
		switch (key) {
		case ' ':
			context.setMirror(!context.mirror());
			break;
		}

		switch (keyCode) {
		case LEFT:
			rotY += 0.1f;
			break;
		case RIGHT:
			rotY -= 0.1f;
			break;
		case UP:
			if(keyEvent.isShiftDown())
				zoomF += 0.02f;
			else
				rotX += 0.1f;
			break;
		case DOWN:
			if(keyEvent.isShiftDown())
				zoomF -= 0.02f;
			else
				rotX -= 0.1f;
			break;
		}
	}
}