From f48a09cde94beccb3a1f9eb2603dc6fc879a3c4d Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Thu, 10 Oct 2013 14:22:06 +0200 Subject: [PATCH 1/2] add freenect_map_rgb_to_depth helper function (untested) --- include/libfreenect-registration.h | 5 +++++ src/registration.c | 29 +++++++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/include/libfreenect-registration.h b/include/libfreenect-registration.h index 7ad2b5bb..0bd86bab 100644 --- a/include/libfreenect-registration.h +++ b/include/libfreenect-registration.h @@ -121,6 +121,11 @@ FREENECTAPI int freenect_destroy_registration(freenect_registration* reg); FREENECTAPI void freenect_camera_to_world(freenect_device* dev, int cx, int cy, int wz, double* wx, double* wy); +// helper function to map one FREENECT_VIDEO_RGB image to a FREENECT_DEPTH_MM +// image (inverse mapping to FREENECT_DEPTH_REGISTERED, which is depth -> RGB) +FREENECTAPI void freenect_map_rgb_to_depth( freenect_device* dev, + uint16_t* depth_mm, uint8_t* rgb_raw, uint8_t* rgb_registered ); + #ifdef __cplusplus } #endif diff --git a/src/registration.c b/src/registration.c index 072b7043..ebcd89f6 100644 --- a/src/registration.c +++ b/src/registration.c @@ -329,6 +329,35 @@ void freenect_camera_to_world(freenect_device* dev, int cx, int cy, int wz, doub *wy = (double)(cy - DEPTH_Y_RES/2) * factor; } +/// RGB -> depth mapping function (inverse of default FREENECT_DEPTH_REGISTERED mapping) +void freenect_map_rgb_to_depth(freenect_device* dev, uint16_t* depth_mm, uint8_t* rgb_raw, uint8_t* rgb_registered) +{ + uint32_t target_offset = dev->registration.reg_pad_info.start_lines * DEPTH_Y_RES; + int x,y; + + for (y = 0; y < DEPTH_Y_RES; y++) for (x = 0; x < DEPTH_X_RES; x++) { + + uint32_t index = y * DEPTH_X_RES + x; + uint32_t cx,cy,cindex; + + int wz = depth_mm[index]; + //if (wz == 0) continue; + + // coordinates in rgb image corresponding to x,y + cx = (dev->registration.registration_table[index][0] + dev->registration.depth_to_rgb_shift[wz]) / REG_X_VAL_SCALE; + cy = dev->registration.registration_table[index][1]; + + if (cx >= DEPTH_X_RES) continue; + + cindex = (cy * DEPTH_X_RES + cx - target_offset) * 3; + index = index*3; + + rgb_registered[index+0] = rgb_raw[cindex+0]; + rgb_registered[index+1] = rgb_raw[cindex+1]; + rgb_registered[index+2] = rgb_raw[cindex+2]; + } +} + /// Allocate and fill registration tables /// This function should be called every time a new video (not depth!) mode is /// activated. From c311f9e794207321a42061b8e47455c0423e82bc Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Thu, 10 Oct 2013 14:22:46 +0200 Subject: [PATCH 2/2] clear rgb pixels without depth data for map_rgb_to_depth --- src/registration.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/registration.c b/src/registration.c index ebcd89f6..c906fecc 100644 --- a/src/registration.c +++ b/src/registration.c @@ -341,9 +341,17 @@ void freenect_map_rgb_to_depth(freenect_device* dev, uint16_t* depth_mm, uint8_t uint32_t cx,cy,cindex; int wz = depth_mm[index]; - //if (wz == 0) continue; - // coordinates in rgb image corresponding to x,y + // pixels without depth data are black + if (wz == 0) { + index = index*3; + rgb_registered[index+0] = 0; + rgb_registered[index+1] = 0; + rgb_registered[index+2] = 0; + continue; + } + + // coordinates in rgb image corresponding to x,y in depth image cx = (dev->registration.registration_table[index][0] + dev->registration.depth_to_rgb_shift[wz]) / REG_X_VAL_SCALE; cy = dev->registration.registration_table[index][1];