diff --git a/fakenect/fakenect.c b/fakenect/fakenect.c index 1b1637a9..b7e01ec7 100644 --- a/fakenect/fakenect.c +++ b/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; diff --git a/src/cameras.c b/src/cameras.c index 834fefb3..a5d580a1 100644 --- a/src/cameras.c +++ b/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 ); diff --git a/src/registration.c b/src/registration.c index 20944890..cbbf557a 100644 --- a/src/registration.c +++ b/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; diff --git a/src/registration.h b/src/registration.h index 8ee23fbf..a2a044e2 100644 --- a/src/registration.h +++ b/src/registration.h @@ -26,10 +26,11 @@ #pragma once +#include #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);