add helper function to convert camera coords -> metric world coords

Signed-off-by: Florian Echtler <[email protected]>

Author: floe

include/libfreenect-registration.h

@@ -117,6 +117,10 @@ typedef struct {
 FREENECTAPI freenect_registration freenect_copy_registration(freenect_device* dev);
 FREENECTAPI int freenect_destroy_registration(freenect_registration* reg);
 
+// convenience function to convert a single x-y coordinate pair from camera
+// to world coordinates
+FREENECTAPI void freenect_camera_to_world(freenect_device* dev,
+	int cx, int cy, int wz, double* wx, double* wy);
 
 #ifdef __cplusplus
 }

src/registration.c

@@ -314,6 +314,16 @@ static void complete_tables(freenect_registration* reg) {
 	freenect_init_registration_table( reg->registration_table, &(reg->reg_info) );
 }
 
+/// camera -> world coordinate helper function
+void freenect_camera_to_world(freenect_device* dev, int cx, int cy, int wz, double* wx, double* wy)
+{
+	double ref_pix_size = dev->registration.zero_plane_info.reference_pixel_size;
+	double ref_distance = dev->registration.zero_plane_info.reference_distance;
+	double factor = ref_pix_size * wz / ref_distance;
+	*wx = (double)(cx - DEPTH_X_RES/2) * factor;
+	*wy = (double)(cy - DEPTH_Y_RES/2) * factor;
+}
+
 /// Allocate and fill registration tables
 /// This function should be called every time a new video (not depth!) mode is
 /// activated.