diff --git a/include/libfreenect-registration.h b/include/libfreenect-registration.h index 2bdb95e8..2ef5d609 100644 --- a/include/libfreenect-registration.h +++ b/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 } diff --git a/src/registration.c b/src/registration.c index 317451b7..33bf4870 100644 --- a/src/registration.c +++ b/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.