nl-knd_client 0.0.0.pre.usegit

data/ext/kinutils/unpack.c

@@ -0,0 +1,212 @@
+/*
+ * Non-inline definitions and functions for Kinect data pack/unpack code.
+ * (C)2011 Mike Bourgeous
+ */
+#include <string.h>
+#include <math.h>
+
+#include <nlutils/nlutils.h>
+
+#define UNPACK_INLINE
+#include "unpack.h"
+
+// TODO: Put ku_xworld, ku_yworld, lut into a library of addons, or submit as patches
+// to libfreenect.
+
+// Depth look-up table (translates depth sample into world-space millimeters).
+int ku_depth_lut[2048];
+
+// Converts x in pixels and z in world millimeters to x in world millimeters.
+static int ku_xworld(int x, int zw)
+{
+	// tan 28 ~= .53171 (1089 ~= .53171 * 2048)
+	// 0xcccd is the ~reciprocal of 10 (factor of W/2=320)
+	// Add 2**34 (0x400000000) for rounding before shift
+	// Shift right by 35:
+	//   11 bits for tangent (* 2048 above)
+	//   19 bits for reciprocal multiplication by 1/10 (factor of W/2=320)
+	//   5 bits for division by 32 (other factor of W/2=320)
+	//
+	// In total, with an overly generous maximum range of +/-16384mm (15
+	// bits), and an overhead of 35 bits due to arithmetic, 50 bits are
+	// needed for the calculation.
+	return (((int64_t)zw * (320 - x) * 1089 * 0xcccd) + 0x400000000) >> 35;
+}
+
+// Converts y in pixels and z in world millimeters to y in world millimeters.
+static int ku_yworld(int y, int zw)
+{
+	return ku_xworld(y + (640 - 480) / 2, zw);
+}
+
+// Initializes the depth look-up table.
+// Copied from the knd daemon code, based on:
+// http://groups.google.com/group/openkinect/browse_thread/thread/31351846fd33c78/e98a94ac605b9f21#e98a94ac605b9f21
+void ku_init_lut()
+{
+	int i;
+
+	for(i = 0; i < 2047; i++) {
+		ku_depth_lut[i] = (int)(0.1236f * tanf(i / 2842.5f + 1.1863f) * 1000.0f);
+	}
+	ku_depth_lut[2047] = 1048576;
+}
+
+// Finds the closest entry in the depth look-up table to the given world-space
+// depth value in millimeters without going over.  Uses a binary search.
+int ku_reverse_lut(int zw)
+{
+	int idx = PXZMAX / 2;
+	int off = PXZMAX / 4;
+
+	while(off > 0 && ku_depth_lut[idx] != zw) {
+		if(ku_depth_lut[idx] > zw) {
+			idx -= off;
+		} else if(ku_depth_lut[idx] < zw) {
+			idx += off;
+		}
+
+		off >>= 1;
+	}
+
+	// Binary search isn't perfect due to truncation, so find the optimum value
+	while(ku_depth_lut[idx] > zw && idx > 0) {
+		idx--;
+	}
+	while(ku_depth_lut[idx + 1] < zw && idx <= PXZMAX) {
+		idx++;
+	}
+
+	return idx;
+}
+
+// Plots a linear depth version of the given perspective image on the 8-bit
+// output surface, which must be 640x480 bytes.
+void plot_linear(const uint16_t *in, uint8_t *out)
+{
+	int x, y, pix;
+
+	for(pix = 0, y = 0; y < 640; y++) {
+		for(x = 0; x < 480; x++, pix++) {
+			out[pix] = 255 - CLAMP(0, 255, ((int32_t)ku_depth_lut[(65535 - in[pix]) >> 5] - 400) * 255 / ZMAX);
+		}
+	}
+}
+
+// Turns overhead view coordinates into a pixel index.
+#define OVPX(xw, zw) (((zw) * ZPIX / ZMAX) * XPIX + ((xw) * XPIX / XMAX + XPIX / 2))
+
+// Plots an overhead view on the given raw linear 8-bit grayscale image
+// surface, which must be XPIX bytes wide by ZPIX bytes tall.
+void plot_overhead(const uint16_t *in, uint8_t *out)
+{
+	int y, x, pix, val;
+	int xw, zw, opx;
+	int c;
+
+	memset(out, 0, XPIX * ZPIX);
+
+	// TODO: Draw borders
+
+	for(pix = 0, y = 0; y < 480; y++) {
+		for(x = 0; x < 640; x++, pix++) {
+			val = (65535 - in[pix]) >> 5;
+			zw = ku_depth_lut[val];
+			if(val >= 2047 || zw >= ZMAX) {
+				continue;
+			}
+			xw = ku_xworld(x, zw);
+
+			opx = OVPX(xw, zw);
+			if(opx < 0 || opx >= XPIX * ZPIX) {
+				continue;
+			}
+			c = out[opx];
+			c += 2;
+			if(c > 255) {
+				c = 255;
+			}
+			out[opx] = c;
+		}
+	}
+}
+
+// Turns side view coordinates into a pixel index.
+#define SVPX(zw, yw) (((-yw) * YPIX / YMAX + YPIX / 2) * ZPIX + ((zw) * ZPIX / ZMAX))
+
+// Plots a side view on the given raw linear 8-bit grayscale image surface,
+// which must be ZPIX bytes wide by YPIX bytes tall.
+void plot_side(const uint16_t *in, uint8_t *out)
+{
+	int y, x, pix, val;
+	int yw, zw, opx;
+	int c;
+
+	memset(out, 0, ZPIX * YPIX);
+
+	// TODO: Draw borders
+	// TODO: Allow skipping for speed
+
+	for(pix = 0, y = 0; y < 480; y++) {
+		for(x = 0; x < 640; x++, pix++) {
+			val = (65535 - in[pix]) >> 5;
+			zw = ku_depth_lut[val];
+			if(val >= 2047 || zw >= ZMAX) {
+				continue;
+			}
+			yw = ku_yworld(y, zw);
+
+			opx = SVPX(zw, yw);
+			if(opx < 0 || opx >= ZPIX * YPIX) {
+				continue;
+			}
+			c = out[opx];
+			c += 2;
+			if(c > 255) {
+				c = 255;
+			}
+			out[opx] = c;
+		}
+	}
+}
+
+// Turns front view coordinates into a pixel index.
+#define FVPX(xw, yw) (((-yw) * YPIX / YMAX + YPIX / 2) * XPIX + ((-xw) * XPIX / XMAX + XPIX / 2))
+
+// Plots a front view on the given raw linear 8-bit grayscale image surface,
+// which must be XPIX bytes wide by YPIX bytes tall.
+void plot_front(const uint16_t *in, uint8_t *out)
+{
+	int y, x, pix, val;
+	int xw, yw, zw, opx;
+	int c;
+
+	memset(out, 0, XPIX * YPIX);
+
+	// TODO: Draw borders
+	// TODO: Allow skipping for speed
+
+	for(pix = 0, y = 0; y < 480; y++) {
+		for(x = 0; x < 640; x++, pix++) {
+			val = (65535 - in[pix]) >> 5;
+			zw = ku_depth_lut[val];
+			if(val >= 2047 || zw >= ZMAX) {
+				continue;
+			}
+			xw = ku_xworld(x, zw);
+			yw = ku_yworld(y, zw);
+
+			opx = FVPX(xw, yw);
+			if(opx < 0 || opx >= XPIX * YPIX) {
+				continue;
+			}
+			c = out[opx];
+			c += 1 + (zw - 512) / 256; // TODO: Scale intensity by surface area
+			if(c > 255) {
+				c = 255;
+			}
+			out[opx] = c;
+		}
+	}
+}
+

data/ext/kinutils/unpack.h

@@ -0,0 +1,108 @@
+/*
+ * Definitions for Kinect data pack/unpack code.
+ * (C)2011 Mike Bourgeous
+ */
+#ifndef UNPACK_H_
+#define UNPACK_H_
+
+#include <stdint.h>
+
+#ifndef UNPACK_INLINE
+#define UNPACK_INLINE inline
+#endif /* UNPACK_INLINE */
+
+// assuming xmax~=3.797 and ymax~=2.848 when z ~= 7
+// calculated as x=5*tan(28)/tan(35) and y=x*3/4
+#define XPIX 500
+#define YPIX 500
+#define ZPIX 500
+#define XMAX 7594 // Actually 2*xmax
+#define YMAX 5696 // Actually 2*ymax
+#define ZMAX 7000
+#define PXZMAX 1092
+
+
+// Depth look-up table (translates depth sample into world-space millimeters).
+extern int ku_depth_lut[2048];
+
+// Unpacks and inverts 8 11-bit pixels (11 bytes) from in and stores them as
+// MSB-aligned 16-bit values in out (16 bytes)
+UNPACK_INLINE void unpack11_to_16(const uint8_t *in, uint16_t *out)
+{
+	out[0] = 65535 - (((in[0] << 3) | (in[1] >> 5)) << 5);
+	out[1] = 65535 - ((((in[1] & 0x1f) << 6) | (in[2] >> 2)) << 5);
+	out[2] = 65535 - ((((in[2] & 0x03) << 9) | (in[3] << 1) | (in[4] >> 7)) << 5);
+	out[3] = 65535 - ((((in[4] & 0x7f) << 4) | (in[5] >> 4)) << 5);
+	out[4] = 65535 - ((((in[5] & 0x0f) << 7) | (in[6] >> 1)) << 5);
+	out[5] = 65535 - ((((in[6] & 0x01) << 10) | (in[7] << 2) | (in[8] >> 6)) << 5);
+	out[6] = 65535 - ((((in[8] & 0x3f) << 5) | (in[9] >> 3)) << 5);
+	out[7] = 65535 - ((((in[9] & 0x07) << 8) | in[10]) << 5);
+}
+
+// Packs 8 LSB-aligned 11-in-16-bit pixels (16 bytes) into 11 output bytes
+UNPACK_INLINE void pack16_to_11(const uint16_t *in, uint8_t *out)
+{
+	out[0] = in[0] >> 3;
+	out[1] = ((in[0] & 0x07) << 5) | (in[1] >> 6);
+	out[2] = ((in[1] & 0x3f) << 2) | (in[2] >> 9);
+	out[3] = (in[2] & 0x1fe) >> 1;
+	out[4] = ((in[2] & 0x01) << 7) | (in[3] >> 4);
+	out[5] = ((in[3] & 0x0f) << 4) | (in[4] >> 7);
+	out[6] = ((in[4] & 0x7f) << 1) | (in[5] >> 10);
+	out[7] = (in[5] & 0x3fc) >> 2;
+	out[8] = ((in[5] & 0x03) << 6) | (in[6] >> 5);
+	out[9] = ((in[6] & 0x1f) << 3) | (in[7] >> 8);
+	out[10] = in[7] & 0xff;
+}
+
+// Unpacks 8 11-bit pixels (11 bytes) into 8 output bytes
+UNPACK_INLINE void unpack11_to_8(const uint8_t *in, uint8_t *out)
+{
+	uint16_t buf16[8];
+	int i;
+
+	unpack11_to_16(in, buf16);
+	for(i = 0; i < 8; i++) {
+		out[i] = buf16[i] >> 3;
+	}
+}
+
+// Unpacks a single pixel into a right-aligned 16-bit value
+UNPACK_INLINE int pxval_11(uint8_t *buf, int pixel)
+{
+	uint32_t byteindex = (pixel * 11) >> 3;
+	uint32_t shiftbits = ((7 + pixel * 5) & 0x7) + 14;
+	uint32_t base;
+
+	buf = buf + byteindex;
+	base = (buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3];
+
+	return (base >> shiftbits) & 0x7ff;
+}
+
+// Initializes the depth look-up table.
+// Copied from the knd daemon code, based on:
+// http://groups.google.com/group/openkinect/browse_thread/thread/31351846fd33c78/e98a94ac605b9f21#e98a94ac605b9f21
+void ku_init_lut();
+
+// Finds the closest entry in the depth look-up table to the given world-space
+// depth value in millimeters without going over.  Uses a binary search.
+int ku_reverse_lut(int zw);
+
+// Plots a linear depth version of the given perspective image on the 8-bit
+// output surface, which must be 640x480 bytes.
+void plot_linear(const uint16_t *in, uint8_t *out);
+
+// Plots an overhead view on the given raw linear 8-bit grayscale image
+// surface, which must be XPIX bytes wide by ZPIX bytes tall.
+void plot_overhead(const uint16_t *in, uint8_t *out);
+
+// Plots a side view on the given raw linear 8-bit grayscale image surface,
+// which must be ZPIX bytes wide by YPIX bytes tall.
+void plot_side(const uint16_t *in, uint8_t *out);
+
+// Plots a front view on the given raw linear 8-bit grayscale image surface,
+// which must be XPIX bytes wide by YPIX bytes tall.
+void plot_front(const uint16_t *in, uint8_t *out);
+
+#endif /* UNPACK_H_ */

data/ext/overhead.c

@@ -0,0 +1,26 @@
+/*
+ * Experimental Kinect depth image unpacking code.
+ * (C)2011 Mike Bourgeous
+ */
+#include <stdio.h>
+#include <stdint.h>
+
+#include "unpack.h"
+
+int main()
+{
+	uint16_t in[640 * 480];
+	uint8_t out[XPIX * ZPIX];
+
+	if(fread(in, 2, 640 * 480, stdin) != 640 * 480) {
+		fprintf(stderr, "Must provide %d bytes of unpacked depth data to stdin.\n", 640 * 480 * 2);
+	}
+
+	ku_init_lut();
+	plot_overhead(in, out);
+
+	fwrite(out, 1, XPIX * ZPIX, stdout);
+	
+	return 0;
+}
+

data/ext/overhead_grid.c

@@ -0,0 +1,42 @@
+/*
+ * Code to render a basic grid for the overhead view, for later manipulation.
+ * The final grid will be created from this program's output using the GIMP.
+ * (C)2012 Mike Bourgeous
+ */
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "unpack.h"
+
+void draw_grid(uint8_t *grid, uint8_t color, int spacing)
+{
+	int x, z;
+
+	// Loop through x in world space, z in pixels
+	for(x = 0; x < XMAX / 2; x += spacing) {
+		for(z = 0; z < ZPIX; z++) {
+			grid[x * XPIX / XMAX + XPIX / 2 + z * XPIX] = color;
+			grid[XPIX / 2 - (x * XPIX / XMAX) + z * XPIX] = color;
+		}
+	}
+
+	// Loop through z in world space, x in pixels
+	for(z = 0; z < ZMAX; z += spacing) {
+		for(x = 0; x < XPIX; x++) {
+			grid[x + z * ZPIX / ZMAX * XPIX] = color;
+		}
+	}
+}
+
+int main()
+{
+	uint8_t out[XPIX * ZPIX];
+
+	memset(out, 0, sizeof(out));
+	draw_grid(out, 128, 500);
+	draw_grid(out, 255, 1000);
+	fwrite(out, 1, sizeof(out), stdout);
+
+	return 0;
+}

data/ext/side.c

@@ -0,0 +1,26 @@
+/*
+ * Experimental Kinect depth image unpacking code.
+ * (C)2011 Mike Bourgeous
+ */
+#include <stdio.h>
+#include <stdint.h>
+
+#include "unpack.h"
+
+int main()
+{
+	uint16_t in[640 * 480];
+	uint8_t out[ZPIX * YPIX];
+
+	if(fread(in, 2, 640 * 480, stdin) != 640 * 480) {
+		fprintf(stderr, "Must provide %d bytes of unpacked depth data to stdin.\n", 640 * 480 * 2);
+	}
+
+	ku_init_lut();
+	plot_side(in, out);
+
+	fwrite(out, 1, sizeof(out), stdout);
+	
+	return 0;
+}
+

data/ext/side_grid.c

@@ -0,0 +1,42 @@
+/*
+ * Code to render a basic grid for the side view, for later manipulation.
+ * The final grid will be created from this program's output using the GIMP.
+ * (C)2012 Mike Bourgeous
+ */
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "unpack.h"
+
+void draw_grid(uint8_t *grid, uint8_t color, int spacing)
+{
+	int y, z;
+
+	// Loop through y in world space, z in piyels
+	for(y = 0; y < YMAX / 2; y += spacing) {
+		for(z = 0; z < ZPIX; z++) {
+			grid[y * YPIX / YMAX * ZPIX + YPIX / 2 * ZPIX + z] = color;
+			grid[YPIX / 2 * ZPIX - (y * YPIX / YMAX * ZPIX) + z] = color;
+		}
+	}
+
+	// Loop through z in world space, y in piyels
+	for(z = 0; z < ZMAX; z += spacing) {
+		for(y = 0; y < YPIX; y++) {
+			grid[y * ZPIX + z * ZPIX / ZMAX] = color;
+		}
+	}
+}
+
+int main()
+{
+	uint8_t out[YPIX * ZPIX];
+
+	memset(out, 0, sizeof(out));
+	draw_grid(out, 128, 500);
+	draw_grid(out, 255, 1000);
+	fwrite(out, 1, sizeof(out), stdout);
+
+	return 0;
+}