From 5c54a0b2c1ff6be2dfc894dbbfedbe7f754df84d Mon Sep 17 00:00:00 2001 From: Benn Snyder Date: Mon, 3 Jan 2022 16:09:44 -0500 Subject: [PATCH] feat: expose a function to apply depth registration - fixes #649 --- CMakeLists.txt | 6 +- examples/camtest.c | 8 +++ fakenect/fakenect.c | 18 +++--- include/libfreenect_registration.h | 12 ++++ src/cameras.c | 2 +- src/registration.c | 100 +++++++++++++++-------------- src/registration.h | 4 +- 7 files changed, 88 insertions(+), 62 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d8525bab..de8ce711 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,8 +46,8 @@ include (FindOS) include (SetupDirectories) set (PROJECT_VER_MAJOR 0) -set (PROJECT_VER_MINOR 6) -set (PROJECT_VER_PATCH 3) +set (PROJECT_VER_MINOR 7) +set (PROJECT_VER_PATCH 0) set (PROJECT_VER "${PROJECT_VER_MAJOR}.${PROJECT_VER_MINOR}.${PROJECT_VER_PATCH}") set (PROJECT_APIVER @@ -101,7 +101,7 @@ SET(DOC_OUTPUT_PATH ${CMAKE_BINARY_DIR}/doc) if (MSVC) set(C_FLAGS_WARNING "-W4") else () - set(C_FLAGS_WARNING "-Wall") + set(C_FLAGS_WARNING "-Wall -Wno-unused-function") endif (MSVC) set(C_CXX_FLAGS_DEFAULT "${C_FLAGS_WARNING} -O2") diff --git a/examples/camtest.c b/examples/camtest.c index 7d796cc1..fc3101d2 100644 --- a/examples/camtest.c +++ b/examples/camtest.c @@ -28,15 +28,23 @@ #include #include #include "libfreenect.h" +#include "libfreenect_registration.h" #ifndef SIGQUIT // win32 compat #define SIGQUIT SIGTERM #endif +uint16_t mapped_depth[640 * 480]; + void depth_cb(freenect_device* dev, void* data, uint32_t timestamp) { printf("Received depth frame at %d\n", timestamp); + + int err = freenect_map_depth_to_video(dev, data, mapped_depth); + if (err) { + printf("Registration error %d\n", err); + } } void video_cb(freenect_device* dev, void* data, uint32_t timestamp) diff --git a/fakenect/fakenect.c b/fakenect/fakenect.c index 8fee220e..eeb7a4b1 100644 --- a/fakenect/fakenect.c +++ b/fakenect/fakenect.c @@ -220,18 +220,20 @@ int freenect_process_events(freenect_context *ctx) best as we can to match those from the original data and current run conditions (e.g., if it takes longer to run this code then we wait less). */ + int err = 0; + if (!index_fp) open_index(); char type; double record_cur_time; unsigned int timestamp, data_size; char *data = NULL; - if (parse_line(&type, &record_cur_time, ×tamp, &data_size, &data)) { - if (loop_playback) { + err = parse_line(&type, &record_cur_time, ×tamp, &data_size, &data); + if (err) { + if (loop_playback) { close_index(); return 0; - } else - return -1; + } else return err; } // Sleep an amount that compensates for the original and current delays // playback_ is w.r.t. the current time @@ -250,9 +252,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_REGISTERED: + err = freenect_apply_registration(&(fake_dev->registration), freenect_find_video_mode(mode.resolution, FREENECT_DEPTH_11BIT), cur_depth, depth_buffer); + break; case FREENECT_DEPTH_MM: freenect_apply_depth_unpacked_to_mm(fake_dev, cur_depth, depth_buffer); break; @@ -303,7 +305,7 @@ int freenect_process_events(freenect_context *ctx) } free(data); playback_prev_time = get_time(); - return 0; + return err; } int freenect_process_events_timeout(freenect_context *ctx, struct timeval *timeout) diff --git a/include/libfreenect_registration.h b/include/libfreenect_registration.h index a17a3437..3f877be9 100644 --- a/include/libfreenect_registration.h +++ b/include/libfreenect_registration.h @@ -125,6 +125,18 @@ FREENECTAPI void freenect_camera_to_world(freenect_device* dev, FREENECTAPI void freenect_map_rgb_to_depth( freenect_device* dev, uint16_t* depth_mm, uint8_t* rgb_raw, uint8_t* rgb_registered ); +/** + * Converts the depth_frame to output_mm and aligns it with the video frame. + * This is similar to using the FREENECT_DEPTH_REGISTERED mode. + * + * @param dev freenect device + * @param depth_frame depth frame buffer + * @param output_mm output buffer which must have length at least width * height + * + * @return 0 on success + */ +FREENECTAPI int freenect_map_depth_to_video(freenect_device* dev, void* depth_frame, uint16_t* output_mm); + #ifdef __cplusplus } #endif diff --git a/src/cameras.c b/src/cameras.c index 6ac5bc04..1c4e2a06 100644 --- a/src/cameras.c +++ b/src/cameras.c @@ -316,7 +316,7 @@ static void depth_process(freenect_device *dev, uint8_t *pkt, int len) convert_packed_to_16bit(dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf, 11, 640*480); break; case FREENECT_DEPTH_REGISTERED: - freenect_apply_registration(dev, dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf, false); + freenect_apply_registration(&(dev->registration), freenect_find_video_mode(dev->depth_resolution, FREENECT_DEPTH_11BIT_PACKED), dev->depth.raw_buf, (uint16_t*)dev->depth.proc_buf); 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 cbbf557a..77bf2379 100644 --- a/src/registration.c +++ b/src/registration.c @@ -27,6 +27,7 @@ #include "libfreenect.h" #include "freenect_internal.h" #include "registration.h" +#include "convert.h" #include #include #include @@ -73,42 +74,23 @@ static void freenect_init_depth_to_rgb(int32_t* depth_to_rgb, freenect_zero_plan } } -// unrolled inner loop of the 11-bit unpacker -static inline void unpack_8_pixels(uint8_t *raw, uint16_t *frame) +// apply registration data to a single frame +FN_INTERNAL int freenect_apply_registration(const freenect_registration* reg, const freenect_frame_mode input_mode, void* input, uint16_t* output_mm) { - uint16_t baseMask = 0x7FF; - - uint8_t r0 = *(raw+0); - uint8_t r1 = *(raw+1); - uint8_t r2 = *(raw+2); - uint8_t r3 = *(raw+3); - uint8_t r4 = *(raw+4); - uint8_t r5 = *(raw+5); - uint8_t r6 = *(raw+6); - uint8_t r7 = *(raw+7); - uint8_t r8 = *(raw+8); - uint8_t r9 = *(raw+9); - uint8_t r10 = *(raw+10); - - frame[0] = (r0<<3) | (r1>>5); - frame[1] = ((r1<<6) | (r2>>2) ) & baseMask; - frame[2] = ((r2<<9) | (r3<<1) | (r4>>7) ) & baseMask; - frame[3] = ((r4<<4) | (r5>>4) ) & baseMask; - frame[4] = ((r5<<7) | (r6>>1) ) & baseMask; - frame[5] = ((r6<<10) | (r7<<2) | (r8>>6) ) & baseMask; - frame[6] = ((r8<<5) | (r9>>3) ) & baseMask; - frame[7] = ((r9<<8) | (r10) ) & baseMask; -} + if (!reg || !input || !output_mm) return -1; + if (!input_mode.is_valid) return -1; + if (input_mode.resolution != FREENECT_RESOLUTION_MEDIUM) return -2; + + if (input_mode.depth_format == FREENECT_DEPTH_REGISTERED) { + memcpy(output_mm, input, input_mode.bytes); + return 0; + } -// apply registration data to a single packed frame -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) size_t i, *wipe = (size_t*)output_mm; for (i = 0; i < DEPTH_X_RES * DEPTH_Y_RES * sizeof(uint16_t) / sizeof(size_t); i++) wipe[i] = DEPTH_NO_MM_VALUE; - uint16_t unpack[8]; + uint16_t unpack[8] = { 0 }; uint32_t target_offset = DEPTH_Y_RES * reg->reg_pad_info.start_lines; uint32_t x,y,source_index = 8; @@ -116,22 +98,32 @@ 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++) { - 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++] ]; - } + uint16_t metric_depth; + + switch (input_mode.depth_format) { + case FREENECT_DEPTH_MM: + uint32_t mm_index = (y * input_mode.width) + x; + metric_depth = ((uint16_t *)input)[mm_index]; + break; + case FREENECT_DEPTH_11BIT: // as used by fakenect-record + case FREENECT_DEPTH_10BIT: // todo: does this work? + uint32_t buf_index = (y * input_mode.width) + x; + metric_depth = reg->raw_to_mm_shift[((uint16_t *)input)[buf_index]]; + break; + case FREENECT_DEPTH_11BIT_PACKED: + case FREENECT_DEPTH_10BIT_PACKED: + // get 8 pixels from the packed frame + if (source_index == 8) { + convert_packed_to_16bit(input, unpack, input_mode.data_bits_per_pixel, 8); + 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++] ]; + break; + default: + return -99; + } // so long as the current pixel has a depth value if (metric_depth == DEPTH_NO_MM_VALUE) continue; @@ -175,17 +167,29 @@ FN_INTERNAL int freenect_apply_registration(freenect_device* dev, uint8_t* input return 0; } +FREENECTAPI int freenect_map_depth_to_video(freenect_device* dev, void* input, uint16_t* output_mm) +{ + if (!dev || !input || !output_mm) return -1; + + if (!dev->registration.registration_table) { + freenect_init_registration(dev); + } + const freenect_registration* reg = &(dev->registration); + const freenect_frame_mode depth_mode = freenect_get_current_depth_mode(dev); + return freenect_apply_registration(reg, depth_mode, input, output_mm); +} + // Same as freenect_apply_registration, but don't bother aligning to the RGB image FN_INTERNAL int freenect_apply_depth_to_mm(freenect_device* dev, uint8_t* input_packed, uint16_t* output_mm) { freenect_registration* reg = &(dev->registration); - uint16_t unpack[8]; + uint16_t unpack[8] = { 0 }; uint32_t x,y,source_index = 8; 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 ); + convert_packed_to_16bit(input_packed, output_mm, 11, 8); source_index = 0; input_packed += 11; } diff --git a/src/registration.h b/src/registration.h index a2a044e2..249f2951 100644 --- a/src/registration.h +++ b/src/registration.h @@ -26,11 +26,11 @@ #pragma once -#include +#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, uint16_t* output_mm, bool unpacked); +int freenect_apply_registration(const freenect_registration* reg, const freenect_frame_mode depth_mode, void* depth_frame, uint16_t* output_mm); 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);