Merge pull request #395 from fran6co/rgb2depth

Add RGB -> depth mapping

Reviewed-by: Benn Snyder <[email protected]>

Author: piedar

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

src/registration.c

@@ -329,6 +329,73 @@ 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;
+	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];
+
+		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] - target_offset;
+
+		if (cx >= DEPTH_X_RES) continue;
+
+		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;
+
+			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
 /// This function should be called every time a new video (not depth!) mode is
 /// activated.