Add FREENECT_DEPTH_REGISTERED support to fakenect (#536)

Reviewed-by: Benn Snyder <benn.snyder@gmail.com>

Author: Cwiiis

fakenect/fakenect.c

@@ -79,6 +79,10 @@ static freenect_frame_mode depth_11_mode =
 	{MAKE_RESERVED(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT),
 	    FREENECT_RESOLUTION_MEDIUM, {FREENECT_DEPTH_11BIT},
 	    640*480*2, 640, 480, 11, 5, 30, 1};
+static freenect_frame_mode depth_registered_mode =
+	{MAKE_RESERVED(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED),
+	    FREENECT_RESOLUTION_MEDIUM, {FREENECT_DEPTH_REGISTERED},
+	    640*480*2, 640, 480, 16, 0, 30, 1};
 static freenect_frame_mode depth_mm_mode =
 	{MAKE_RESERVED(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_MM),
 	    FREENECT_RESOLUTION_MEDIUM, {FREENECT_DEPTH_MM},
@@ -246,6 +250,9 @@ int freenect_process_events(freenect_context *ctx)
 				case FREENECT_DEPTH_11BIT:
 				    memcpy(depth_buffer, cur_depth, mode.bytes);
 				    break;
+                                case FREENECT_DEPTH_REGISTERED:
+                                    freenect_apply_registration(fake_dev, cur_depth, depth_buffer, true);
+                                    break;
 				case FREENECT_DEPTH_MM:
 				    freenect_apply_depth_unpacked_to_mm(fake_dev, cur_depth, depth_buffer);
 				    break;
@@ -353,7 +360,8 @@ int freenect_set_depth_mode(freenect_device* dev, const freenect_frame_mode mode
         // underlying data.  Would be better to check for conflict.
 	depth_mode = mode;
 
-	if (mode.depth_format == FREENECT_DEPTH_MM  &&
+	if ((mode.depth_format == FREENECT_DEPTH_MM ||
+             mode.depth_format == FREENECT_DEPTH_REGISTERED) &&
 	    dev->registration.zero_plane_info.reference_distance == 0) {
 		printf("Warning: older fakenect recording doesn't contain "
 		       "registration info for mapping depth to MM units\n");
@@ -407,6 +415,8 @@ freenect_frame_mode freenect_find_depth_mode(freenect_resolution res, freenect_d
     switch (fmt) {
     case FREENECT_DEPTH_11BIT:
 	    return depth_11_mode;
+    case FREENECT_DEPTH_REGISTERED:
+            return depth_registered_mode;
     case FREENECT_DEPTH_MM:
 	    return depth_mm_mode;
     default:
@@ -420,7 +430,7 @@ freenect_frame_mode freenect_find_depth_mode(freenect_resolution res, freenect_d
 
 int freenect_get_depth_mode_count()
 {
-    return 1;
+    return 3;
 }
 
 freenect_frame_mode freenect_get_depth_mode(int mode_num)
@@ -429,6 +439,8 @@ freenect_frame_mode freenect_get_depth_mode(int mode_num)
 	return depth_11_mode;
     else if (mode_num == 1)
 	return depth_mm_mode;
+    else if (mode_num == 2)
+        return depth_registered_mode;
     else {
 	freenect_frame_mode invalid = { 0 };
 	return invalid;

src/cameras.c

@@ -397,7 +397,7 @@ static void depth_process(freenect_device *dev, uint8_t *pkt, int len)
 			convert_packed11_to_16bit(dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf, 640*480);
 			break;
 		case FREENECT_DEPTH_REGISTERED:
-			freenect_apply_registration(dev, dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf );
+			freenect_apply_registration(dev, dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf, false);
 			break;
 		case FREENECT_DEPTH_MM:
 			freenect_apply_depth_to_mm(dev, dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf );

src/registration.c

@@ -101,7 +101,7 @@ static inline void unpack_8_pixels(uint8_t *raw, uint16_t *frame)
 }
 
 // apply registration data to a single packed frame
-FN_INTERNAL int freenect_apply_registration(freenect_device* dev, uint8_t* input_packed, uint16_t* output_mm)
+FN_INTERNAL int freenect_apply_registration(freenect_device* dev, uint8_t* input, uint16_t* output_mm, bool unpacked)
 {
 	freenect_registration* reg = &(dev->registration);
 	// set output buffer to zero using pointer-sized memory access (~ 30-40% faster than memset)
@@ -116,15 +116,22 @@ FN_INTERNAL int freenect_apply_registration(freenect_device* dev, uint8_t* input
 	for (y = 0; y < DEPTH_Y_RES; y++) {
 		for (x = 0; x < DEPTH_X_RES; x++) {
 
-			// get 8 pixels from the packed frame
-			if (source_index == 8) {
-				unpack_8_pixels( input_packed, unpack );
-				source_index = 0;
-				input_packed += 11;
-			}
-
-			// get the value at the current depth pixel, convert to millimeters
-			uint16_t metric_depth = reg->raw_to_mm_shift[ unpack[source_index++] ];
+                        uint16_t metric_depth;
+
+                        if (unpacked) {
+                                uint32_t buf_index = y * DEPTH_X_RES + x;
+                                metric_depth = reg->raw_to_mm_shift[((uint16_t *)input)[buf_index]];
+                        } else {
+                                // get 8 pixels from the packed frame
+                                if (source_index == 8) {
+                                        unpack_8_pixels( input, unpack );
+                                        source_index = 0;
+                                        input += 11;
+                                }
+
+                                // get the value at the current depth pixel, convert to millimeters
+                                metric_depth = reg->raw_to_mm_shift[ unpack[source_index++] ];
+                        }
 
 			// so long as the current pixel has a depth value
 			if (metric_depth == DEPTH_NO_MM_VALUE) continue;

src/registration.h

@@ -26,10 +26,11 @@
 
 #pragma once
 
+#include <stdbool.h>
 #include "libfreenect.h"
 
 // Internal function declarations relating to registration
 int freenect_init_registration(freenect_device* dev);
-int freenect_apply_registration(freenect_device* dev, uint8_t* input_packed, uint16_t* output_mm);
+int freenect_apply_registration(freenect_device* dev, uint8_t* input, uint16_t* output_mm, bool unpacked);
 int freenect_apply_depth_to_mm(freenect_device* dev, uint8_t* input_packed, uint16_t* output_mm);
 int freenect_apply_depth_unpacked_to_mm(freenect_device* dev, uint16_t* input, uint16_t* output_mm);