From a55cd437f00df1bc2101a86098a8cd3fae291ee4 Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Thu, 10 Oct 2013 14:22:06 +0200 Subject: [PATCH 1/3] 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 43aeb973..a17a3437 100644 --- a/include/libfreenect_registration.h +++ b/include/libfreenect_registration.h @@ -120,6 +120,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 7111ae1f..f581ad50 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 be35d02d1d893981f943e45424e7e87d2161ef4c Mon Sep 17 00:00:00 2001 From: Florian Echtler Date: Thu, 10 Oct 2013 14:22:46 +0200 Subject: [PATCH 2/3] 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 f581ad50..6c9833fb 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]; From 9069ddf4f7714d79484a6c455c649a7be64b5b6c Mon Sep 17 00:00:00 2001 From: Francisco Facioni Date: Mon, 9 Jun 2014 19:51:59 -0300 Subject: [PATCH 3/3] Adds zbuffer to the mapping of rgb to depth Signed-off-by: Francisco Facioni --- src/registration.c | 58 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 44 insertions(+), 14 deletions(-) diff --git a/src/registration.c b/src/registration.c index 6c9833fb..efd38645 100644 --- a/src/registration.c +++ b/src/registration.c @@ -334,36 +334,66 @@ void freenect_map_rgb_to_depth(freenect_device* dev, uint16_t* depth_mm, uint8_t { uint32_t target_offset = dev->registration.reg_pad_info.start_lines * DEPTH_Y_RES; int x,y; + int* map = (int*)malloc(DEPTH_Y_RES*DEPTH_X_RES* sizeof(int)); + unsigned short* zBuffer = (unsigned short*)malloc(DEPTH_Y_RES*DEPTH_X_RES* sizeof(unsigned short)); + memset(zBuffer, DEPTH_NO_MM_VALUE, DEPTH_X_RES*DEPTH_Y_RES * sizeof(unsigned short)); 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; + map[index] = -1; + int wz = depth_mm[index]; - // 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; + if (wz == DEPTH_NO_MM_VALUE) { 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]; + 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] - target_offset; if (cx >= DEPTH_X_RES) continue; - cindex = (cy * DEPTH_X_RES + cx - target_offset) * 3; - index = index*3; + cindex = cy*DEPTH_X_RES+cx; + map[index] = cindex; + + if (zBuffer[cindex] == DEPTH_NO_MM_VALUE || zBuffer[cindex] > wz) { + zBuffer[cindex] = wz; + } + } + + 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 cindex = map[index]; + + // pixels without depth data or out of bounds are black + if (cindex == -1) { + index *= 3; + rgb_registered[index+0] = 0; + rgb_registered[index+1] = 0; + rgb_registered[index+2] = 0; - rgb_registered[index+0] = rgb_raw[cindex+0]; - rgb_registered[index+1] = rgb_raw[cindex+1]; - rgb_registered[index+2] = rgb_raw[cindex+2]; + continue; + } + + unsigned short currentDepth = depth_mm[index]; + unsigned short minDepth = zBuffer[cindex]; + + // filters out pixels that are occluded + if (currentDepth <= minDepth) { + index *= 3; + cindex *= 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]; + } } + + free(zBuffer); + free(map); } /// Allocate and fill registration tables