From e8fd62c8f2e893089df581b4299aedc3f46d9c93 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Mon, 9 Apr 2012 22:47:42 -0400 Subject: [PATCH 01/11] offline registration --- examples/kinect_register.c | 379 +++++++++++++++++++++++++++++++++++++ 1 file changed, 379 insertions(+) create mode 100644 examples/kinect_register.c diff --git a/examples/kinect_register.c b/examples/kinect_register.c new file mode 100644 index 00000000..b5e9732c --- /dev/null +++ b/examples/kinect_register.c @@ -0,0 +1,379 @@ +/* + * This file is part of the OpenKinect Project. http://www.openkinect.org + * + * Copyright (c) 2010 individual OpenKinect contributors. See the CONTRIB file + * for details. + * + * This code is licensed to you under the terms of the Apache License, version + * 2.0, or, at your option, the terms of the GNU General Public License, + * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses, + * or the following URLs: + * http://www.apache.org/licenses/LICENSE-2.0 + * http://www.gnu.org/licenses/gpl-2.0.txt + * + * If you redistribute this file in source form, modified or unmodified, you + * may: + * 1) Leave this header intact and distribute it under the same terms, + * accompanying it with the APACHE20 and GPL20 files, or + * 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or + * 3) Delete the GPL v2 clause and accompany it with the APACHE20 file + * In all cases you must keep the copyright notice intact and include a copy + * of the CONTRIB file. + * + * Binary distributions must follow the binary distribution requirements of + * either License. + * + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#define DEPTH_MAX_METRIC_VALUE 10000 +#define DEPTH_MAX_RAW_VALUE 2048 +#define DEPTH_NO_MM_VALUE 0 + +#define DEPTH_X_RES 640 +#define DEPTH_Y_RES 480 + +#define REG_X_VAL_SCALE 256 // "fixed-point" precision for double -> int32_t conversion +#define DEPTH_MIRROR_X 0 + +void dump_registration(char* regfile) { + printf("Dumping Kinect registration to %s\n", regfile); + + freenect_context *f_ctx; + if (freenect_init(&f_ctx, NULL) < 0) { + printf("freenect_init() failed\n"); + exit(0); + //return 1; + } + + freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG); + freenect_select_subdevices(f_ctx, + (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); + + freenect_device *f_dev; + int user_device_number = 0; + if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) { + printf("Could not open device\n"); + exit(0); + //return 1; + } + + freenect_registration dev; + dev = freenect_copy_registration( f_dev ); + + FILE* fp = fopen(regfile, "w"); + if (!fp) { + printf("Error: Cannot open file '%s'\n", regfile); + exit(1); + } + + // from freenect_copy_registration in registration.c + /* + dev.reg_info = dev->registration.reg_info; + dev.reg_pad_info = dev->registration.reg_pad_info; + dev.zero_plane_info = dev->registration.zero_plane_info; + dev.const_shift = dev->registration.const_shift; + dev.raw_to_mm_shift = (uint16_t*)malloc( sizeof(uint16_t) * DEPTH_MAX_RAW_VALUE ); + dev.depth_to_rgb_shift = (int32_t*)malloc( sizeof( int32_t) * DEPTH_MAX_METRIC_VALUE ); + dev.registration_table = (int32_t (*)[2])malloc( sizeof( int32_t) * DEPTH_X_RES * DEPTH_Y_RES * 2 ); + */ + + /* Should be ~2.4 MB + printf( "\n\n%d\n\n", sizeof(dev)+ + sizeof(uint16_t)*DEPTH_MAX_RAW_VALUE + + sizeof(int32_t)*DEPTH_MAX_METRIC_VALUE + + sizeof(int32_t)*DEPTH_X_RES*DEPTH_Y_RES*2 ); + */ + + // write out first four fields + fwrite( &dev.reg_info, sizeof(dev.reg_info), 1, fp ); + fwrite( &dev.reg_pad_info, sizeof(dev.reg_pad_info), 1, fp); + fwrite( &dev.zero_plane_info, sizeof(dev.zero_plane_info), 1, fp); + fwrite( &dev.const_shift, sizeof(dev.const_shift), 1, fp); + + //printf( "\n\n%d\n\n", sizeof(uint16_t)*DEPTH_MAX_RAW_VALUE ); // 4096 + fwrite( (&dev)->raw_to_mm_shift, sizeof(uint16_t), DEPTH_MAX_RAW_VALUE, fp ); + + //printf( "\n\n%d\n\n", sizeof(int32_t)*DEPTH_MAX_METRIC_VALUE ); // 4000 + fwrite( (&dev)->depth_to_rgb_shift, sizeof(int32_t), DEPTH_MAX_METRIC_VALUE, fp ); + + //printf( "\n\n%d\n\n", sizeof(int32_t)*DEPTH_X_RES*DEPTH_Y_RES*2 ); // 2457600 + fwrite( (&dev)->registration_table, sizeof(int32_t), DEPTH_X_RES*DEPTH_Y_RES*2, fp ); + + fclose(fp); +} + + +// From: https://github.com/floe/libfreenect/blob/ee8666614e9ff620f49c83f466e5a99f6a0be4a0/src/registration.c +int freenect_apply_registration(freenect_registration* reg, uint16_t* input_raw, uint16_t* output_mm) { + + memset(output_mm, DEPTH_NO_MM_VALUE, DEPTH_X_RES * DEPTH_Y_RES * sizeof(uint16_t)); // clear the output image + uint32_t target_offset = DEPTH_Y_RES * reg->reg_pad_info.start_lines; + uint32_t x,y,source_index = 0; + + for (y = 0; y < DEPTH_Y_RES; y++) { + for (x = 0; x < DEPTH_X_RES; x++) { + + // get the value at the current depth pixel, convert to millimeters + uint16_t metric_depth = reg->raw_to_mm_shift[ input_raw[source_index++] ]; + + // so long as the current pixel has a depth value + if (metric_depth == DEPTH_NO_MM_VALUE) continue; + if (metric_depth >= DEPTH_MAX_METRIC_VALUE) continue; + + // calculate the new x and y location for that pixel + // using registration_table for the basic rectification + // and depth_to_rgb_shift for determining the x shift + uint32_t reg_index = DEPTH_MIRROR_X ? ((y + 1) * DEPTH_X_RES - x - 1) : (y * DEPTH_X_RES + x); + uint32_t nx = (reg->registration_table[reg_index][0] + reg->depth_to_rgb_shift[metric_depth]) / REG_X_VAL_SCALE; + uint32_t ny = reg->registration_table[reg_index][1]; + + // ignore anything outside the image bounds + if (nx >= DEPTH_X_RES) continue; + + // convert nx, ny to an index in the depth image array + uint32_t target_index = (DEPTH_MIRROR_X ? ((ny + 1) * DEPTH_X_RES - nx - 1) : (ny * DEPTH_X_RES + nx)) - target_offset; + + // get the current value at the new location + uint16_t current_depth = output_mm[target_index]; + + // make sure the new location is empty, or the new value is closer + if ((current_depth == DEPTH_NO_MM_VALUE) || (current_depth > metric_depth)) { + output_mm[target_index] = metric_depth; // always save depth at current location + } + } + } + return 0; +} + +freenect_registration load_registration(char* regfile) +{ + + FILE *fp = NULL; + /* load the regdump file */ + fp = fopen(regfile, "r"); + if (!fp) { + perror(regfile); + exit(1); + } + + freenect_registration reg; + // allocate memory and set up pointers + // (from freenect_copy_registration in registration.c) + reg.raw_to_mm_shift = (uint16_t*)malloc( sizeof(uint16_t) * DEPTH_MAX_RAW_VALUE ); + reg.depth_to_rgb_shift = (int32_t*)malloc( sizeof( int32_t) * DEPTH_MAX_METRIC_VALUE ); + reg.registration_table = (int32_t (*)[2])malloc( sizeof( int32_t) * DEPTH_X_RES * DEPTH_Y_RES * 2 ); + // load (inverse of kinect_regdump) + fread( ®.reg_info, sizeof(reg.reg_info), 1, fp ); + fread( ®.reg_pad_info, sizeof(reg.reg_pad_info), 1, fp); + fread( ®.zero_plane_info, sizeof(reg.zero_plane_info), 1, fp); + fread( ®.const_shift, sizeof(reg.const_shift), 1, fp); + fread( reg.raw_to_mm_shift, sizeof(uint16_t), DEPTH_MAX_RAW_VALUE, fp ); + fread( reg.depth_to_rgb_shift, sizeof(int32_t), DEPTH_MAX_METRIC_VALUE, fp ); + fread( reg.registration_table, sizeof(int32_t), DEPTH_X_RES*DEPTH_Y_RES*2, fp ); + fclose(fp); + + return reg; +} + + +void load_PGM(char *PGMfile, uint16_t* data) +{ + FILE *fp = NULL; + fp = fopen(PGMfile, "r"); + if (!fp) { + perror(PGMfile); + exit(1); + } + while (getc(fp) != '\n'); // skip header line + fread(data, sizeof(uint16_t), DEPTH_X_RES*DEPTH_Y_RES, fp); + fclose(fp); +} + +/* +void write_PGM(char *PGMfile, uint16_t* data, char *type) +{ + // make filename .dist.pgm + char *PGMfile_out = (char *)malloc(strlen(PGMfile) + 2); // '.x' + sprintf(PGMfile_out, "%s", PGMfile); + sprintf(PGMfile_out+strlen(PGMfile)-3, "%s", type); + sprintf(PGMfile_out+strlen(PGMfile)-2, "%s", ".pgm"); + + FILE *fp = NULL; + fp = fopen(PGMfile_out, "w"); + if (!fp) { + perror(PGMfile_out); + exit(1); + } + + fprintf(fp, "P5 %d %d 65535\n", DEPTH_X_RES, DEPTH_Y_RES ); + fwrite(data, sizeof(uint16_t), DEPTH_X_RES*DEPTH_Y_RES, fp); + fclose(fp); +} +*/ + + +void write_xyz_bin(char *infile, double* x, double* y, uint16_t* z) +{ + FILE* fp; + // make filename .x + char *file_out = (char *)malloc(strlen(infile) + 10); + sprintf(file_out, "%s", infile); + + // X + sprintf(file_out+strlen(infile)-3, "%s", "x"); + fp = NULL; + fp = fopen(file_out, "w"); + if (!fp) { + perror(file_out); + exit(1); + } + fwrite(x, sizeof(double), DEPTH_X_RES*DEPTH_Y_RES, fp); + fclose(fp); + + // Y + sprintf(file_out+strlen(infile)-3, "%s", "y"); + fp = NULL; + fp = fopen(file_out, "w"); + if (!fp) { + perror(file_out); + exit(1); + } + fwrite(y, sizeof(double), DEPTH_X_RES*DEPTH_Y_RES, fp); + fclose(fp); + + // Z + sprintf(file_out+strlen(infile)-3, "%s", "z"); + fp = NULL; + fp = fopen(file_out, "w"); + if (!fp) { + perror(file_out); + exit(1); + } + fwrite(z, sizeof(uint16_t), DEPTH_X_RES*DEPTH_Y_RES, fp); + fclose(fp); +} + + + +void apply_registration(char* regfile, char* PGMfile) +{ + int i, j; + + freenect_registration reg; + reg = load_registration(regfile); + + uint16_t data[DEPTH_X_RES*DEPTH_Y_RES]; + load_PGM(PGMfile, data); + + /* Convert DN to world */ + // first, convert the DN to worldz + double wx[DEPTH_X_RES*DEPTH_Y_RES], wy[DEPTH_X_RES*DEPTH_Y_RES]; + uint16_t wz[DEPTH_X_RES*DEPTH_Y_RES]; + freenect_apply_registration(®, data, wz); + + // then, convert x and y to world_x and world_y + printf("Converting camera to world coordinates\n"); + + // see freenect_camera_to_world() in registration.c + double ref_pix_size = reg.zero_plane_info.reference_pixel_size; + double ref_distance = reg.zero_plane_info.reference_distance; + double factor; + uint32_t idx = 0; + for (j=0; j | -a \n"); + printf(" -h: Display this help message\n" + " -s: Save the registration parameters from a connected Kinect to \n" + " -a: Apply the registration parameters from to (from 'record')\n" + ); + exit(0); +} + +int main(int argc, char **argv) +{ + int c=1; + char *regfile=0, *PGMfile=0; + if (argc == 1) + usage(); + + while (c < argc) { + if (strcmp(argv[c],"-h")==0) + usage(); + else if (strcmp(argv[c],"-s")==0) { + if (argc <= (c+1)) + usage(); + regfile = argv[c+1]; + } + else if (strcmp(argv[c],"-a")==0) { + if (argc <= (c+2)) + usage(); + regfile = argv[c+1]; + PGMfile = argv[c+2]; + } + c++; + } + + if (regfile != 0 && PGMfile == 0) + dump_registration(regfile); + else + apply_registration(regfile, PGMfile); + + return 0; +} From b5ae7f1f833ba62f7d18ae384f88b5dc57a4466c Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Mon, 9 Apr 2012 22:51:17 -0400 Subject: [PATCH 02/11] Build and install offline registration --- examples/CMakeLists.txt | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 13c98fc4..675767d1 100755 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -20,6 +20,7 @@ endif() add_executable(glview glview.c) add_executable(regview regview.c) add_executable(hiview hiview.c) +add_executable(kinect_register kinect_register.c) if(BUILD_AUDIO) add_executable(wavrecord wavrecord.c) add_executable(micview micview.c) @@ -40,6 +41,7 @@ if(APPLE) target_link_libraries(glview freenect) target_link_libraries(regview freenect) target_link_libraries(hiview freenect) + target_link_libraries(kinect_register freenect) if (BUILD_AUDIO) target_link_libraries(wavrecord freenect) target_link_libraries(micview freenect) @@ -61,6 +63,7 @@ else() target_link_libraries(glview freenect ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${MATH_LIB}) target_link_libraries(regview freenect ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${MATH_LIB}) target_link_libraries(hiview freenect ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${MATH_LIB}) + target_link_libraries(kinect_register freenect ${CMAKE_THREAD_LIBS_INIT}) if (BUILD_AUDIO) target_link_libraries(wavrecord freenect ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${MATH_LIB}) target_link_libraries(micview freenect ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${MATH_LIB}) @@ -74,7 +77,7 @@ else() endif() -install (TARGETS glview regview hiview +install (TARGETS glview regview hiview kinect_register DESTINATION bin) if (BUILD_C_SYNC) From 355c600225794353de3866623b346c452c874982 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Thu, 12 Apr 2012 12:43:47 -0400 Subject: [PATCH 03/11] Less Verbose. Better filenames --- examples/kinect_register.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index b5e9732c..7895d6f2 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -284,7 +284,7 @@ void apply_registration(char* regfile, char* PGMfile) freenect_apply_registration(®, data, wz); // then, convert x and y to world_x and world_y - printf("Converting camera to world coordinates\n"); + //printf("Converting camera to world coordinates\n"); // see freenect_camera_to_world() in registration.c double ref_pix_size = reg.zero_plane_info.reference_pixel_size; @@ -307,9 +307,10 @@ void apply_registration(char* regfile, char* PGMfile) write_xyz_bin(PGMfile, wx, wy, wz); /* Write out x,y,z ASCII file */ - printf("Writing xyz file\n"); + //printf("Writing xyz file\n"); char *outfile_ascii = (char *)malloc(strlen(PGMfile) + 3); - sprintf(outfile_ascii, "%s.xyz", PGMfile ); // append '.xyz' to input filename + sprintf(outfile_ascii, "%s", PGMfile ); // append '.xyz' to input filename + sprintf(outfile_ascii+strlen(PGMfile)-3, "%s", "xyz"); FILE *fp; fp = fopen(outfile_ascii, "w"); if (!fp) { @@ -328,7 +329,7 @@ void apply_registration(char* regfile, char* PGMfile) fprintf(fp, "%d %d %d\n", (int16_t)wx[i], (int16_t)wy[i], wz[i]); } fclose(fp); - printf("Wrote xyz file\n"); + //printf("Wrote xyz file\n"); } From f7123cd6ab3cc71ada011416e03118e264fdbc82 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Sun, 15 Apr 2012 19:34:23 +0200 Subject: [PATCH 04/11] depth registration without shifting to RGB coordinates --- examples/kinect_register.c | 61 +++++++++++++++++++++++++++++++++++--- 1 file changed, 57 insertions(+), 4 deletions(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index 7895d6f2..50f128f3 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -47,6 +47,12 @@ #define REG_X_VAL_SCALE 256 // "fixed-point" precision for double -> int32_t conversion #define DEPTH_MIRROR_X 0 + +#define S2D_PIXEL_CONST 10 +#define S2D_CONST_OFFSET 0.375 + + + void dump_registration(char* regfile) { printf("Dumping Kinect registration to %s\n", regfile); @@ -201,7 +207,6 @@ void load_PGM(char *PGMfile, uint16_t* data) fclose(fp); } -/* void write_PGM(char *PGMfile, uint16_t* data, char *type) { // make filename .dist.pgm @@ -221,7 +226,6 @@ void write_PGM(char *PGMfile, uint16_t* data, char *type) fwrite(data, sizeof(uint16_t), DEPTH_X_RES*DEPTH_Y_RES, fp); fclose(fp); } -*/ void write_xyz_bin(char *infile, double* x, double* y, uint16_t* z) @@ -266,10 +270,24 @@ void write_xyz_bin(char *infile, double* x, double* y, uint16_t* z) } +/* +/// convert raw shift value to metric depth (in mm) +static uint16_t freenect_raw_to_mm(uint16_t raw, freenect_registration* reg) +{ + double parameter_coefficient = 4; + double shift_scale = 10; + double pixel_size_factor = 1; + freenect_zero_plane_info* zpi = &(reg->zero_plane_info); + double fixed_ref_x = ((raw - (parameter_coefficient * reg->const_shift / pixel_size_factor)) / parameter_coefficient) - S2D_CONST_OFFSET; + double metric = fixed_ref_x * zpi->reference_pixel_size * pixel_size_factor; + return shift_scale * ((metric * zpi->reference_distance / (zpi->dcmos_emitter_dist - metric)) + zpi->reference_distance); +} +*/ void apply_registration(char* regfile, char* PGMfile) { int i, j; + int x, y; freenect_registration reg; reg = load_registration(regfile); @@ -277,12 +295,47 @@ void apply_registration(char* regfile, char* PGMfile) uint16_t data[DEPTH_X_RES*DEPTH_Y_RES]; load_PGM(PGMfile, data); + // freenect_apply_depth_to_mm from registration.c + // This converts the raw data to depth data, but does not mess with + // pixel alignment. + uint16_t wz[DEPTH_X_RES*DEPTH_Y_RES]; + for (y = 0; y < DEPTH_Y_RES; y++) { + for (x = 0; x < DEPTH_X_RES; x++) { + // get the value at the current depth pixel, convert to millimeters + uint16_t metric_depth = (®)->raw_to_mm_shift[ data[y*DEPTH_X_RES+x] ]; + wz[y * DEPTH_X_RES + x] = metric_depth < DEPTH_MAX_METRIC_VALUE ? metric_depth : DEPTH_MAX_METRIC_VALUE; + } + } + write_PGM( PGMfile, wz, "d" ); + // inputfile.pgm -> inputfile.d.pgm + /* Convert DN to world */ // first, convert the DN to worldz double wx[DEPTH_X_RES*DEPTH_Y_RES], wy[DEPTH_X_RES*DEPTH_Y_RES]; - uint16_t wz[DEPTH_X_RES*DEPTH_Y_RES]; - freenect_apply_registration(®, data, wz); + //uint16_t wz[DEPTH_X_RES*DEPTH_Y_RES]; + // freenect_map_rgd_to_depth from registration.c + /* + uint32_t target_offset = dev->registration.reg_pad_info.start_lines * DEPTH_Y_RES; + 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; + wz[index] = depth_mm[index]; + // 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]; + } + */ + + //freenect_apply_registration(®, data, wz); + //wz = output_mm; + // then, convert x and y to world_x and world_y //printf("Converting camera to world coordinates\n"); From cbea3cadb716974c786797f553620b90358e5d58 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Sun, 15 Apr 2012 23:54:18 +0200 Subject: [PATCH 05/11] RGB mapped onto depth --- examples/kinect_register.c | 124 ++++++++++++++++++++++++------------- 1 file changed, 82 insertions(+), 42 deletions(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index 50f128f3..7f236393 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -122,6 +122,7 @@ void dump_registration(char* regfile) { // From: https://github.com/floe/libfreenect/blob/ee8666614e9ff620f49c83f466e5a99f6a0be4a0/src/registration.c +/* int freenect_apply_registration(freenect_registration* reg, uint16_t* input_raw, uint16_t* output_mm) { memset(output_mm, DEPTH_NO_MM_VALUE, DEPTH_X_RES * DEPTH_Y_RES * sizeof(uint16_t)); // clear the output image @@ -162,6 +163,8 @@ int freenect_apply_registration(freenect_registration* reg, uint16_t* input_raw, } return 0; } +*/ + freenect_registration load_registration(char* regfile) { @@ -207,6 +210,40 @@ void load_PGM(char *PGMfile, uint16_t* data) fclose(fp); } +void load_PPM(char *PPMfile, uint8_t* data) +{ + // written like this: + //fprintf(fp, "P6 %d %d 255\n", FREENECT_FRAME_W, FREENECT_FRAME_H); + //fwrite(data, data_size, 1, fp); + FILE *fp = NULL; + fp = fopen(PPMfile, "r"); + if (!fp) { + perror(PPMfile); + exit(1); + } + while (getc(fp) != '\n'); // skip P6 header + fread(data, sizeof(uint8_t), DEPTH_X_RES*DEPTH_Y_RES*3, fp); +} + +void write_PPM(char *PPMfile, uint8_t* data) +{ + char *PPMfile_out = (char *)malloc(strlen(PPMfile) + 4); // ".reg" + sprintf(PPMfile_out, "%s", PPMfile); + sprintf(PPMfile_out+strlen(PPMfile)-4, "%s", ".reg"); + sprintf(PPMfile_out+strlen(PPMfile), "%s", ".ppm"); + + FILE *fp = NULL; + fp = fopen(PPMfile_out, "w"); + if (!fp) { + perror(PPMfile_out); + exit(1); + } + + fprintf(fp, "P6 %d %d 255\n", DEPTH_X_RES, DEPTH_Y_RES); + fwrite(data, DEPTH_X_RES*DEPTH_Y_RES*3, 1, fp); + fclose(fp); +} + void write_PGM(char *PGMfile, uint16_t* data, char *type) { // make filename .dist.pgm @@ -269,22 +306,36 @@ void write_xyz_bin(char *infile, double* x, double* y, uint16_t* z) fclose(fp); } - -/* -/// convert raw shift value to metric depth (in mm) -static uint16_t freenect_raw_to_mm(uint16_t raw, freenect_registration* reg) +/// RGB -> depth mapping function (inverse of default FREENECT_DEPTH_REGISTERED mapping) +void freenect_map_rgb_to_depth(freenect_registration* reg, uint16_t* depth_mm, uint8_t* rgb_raw, uint8_t* rgb_registered) { - double parameter_coefficient = 4; - double shift_scale = 10; - double pixel_size_factor = 1; - freenect_zero_plane_info* zpi = &(reg->zero_plane_info); - double fixed_ref_x = ((raw - (parameter_coefficient * reg->const_shift / pixel_size_factor)) / parameter_coefficient) - S2D_CONST_OFFSET; - double metric = fixed_ref_x * zpi->reference_pixel_size * pixel_size_factor; - return shift_scale * ((metric * zpi->reference_distance / (zpi->dcmos_emitter_dist - metric)) + zpi->reference_distance); + uint32_t target_offset = reg->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 = (reg->registration_table[index][0] + reg->depth_to_rgb_shift[wz]) / REG_X_VAL_SCALE; + cy = reg->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]; + } } -*/ -void apply_registration(char* regfile, char* PGMfile) +void apply_registration(char* regfile, char* PGMfile, char* PPMfile) { int i, j; int x, y; @@ -295,10 +346,18 @@ void apply_registration(char* regfile, char* PGMfile) uint16_t data[DEPTH_X_RES*DEPTH_Y_RES]; load_PGM(PGMfile, data); + uint8_t data_ppm[DEPTH_X_RES*DEPTH_Y_RES*3]; + uint8_t data_ppm_reg[DEPTH_X_RES*DEPTH_Y_RES*3]; + if (PPMfile != NULL) { + load_PPM(PPMfile, data_ppm); + // write_PPM(PPMfile, data_ppm); + } + // freenect_apply_depth_to_mm from registration.c // This converts the raw data to depth data, but does not mess with // pixel alignment. uint16_t wz[DEPTH_X_RES*DEPTH_Y_RES]; + double wx[DEPTH_X_RES*DEPTH_Y_RES], wy[DEPTH_X_RES*DEPTH_Y_RES]; for (y = 0; y < DEPTH_Y_RES; y++) { for (x = 0; x < DEPTH_X_RES; x++) { // get the value at the current depth pixel, convert to millimeters @@ -311,33 +370,9 @@ void apply_registration(char* regfile, char* PGMfile) /* Convert DN to world */ // first, convert the DN to worldz - double wx[DEPTH_X_RES*DEPTH_Y_RES], wy[DEPTH_X_RES*DEPTH_Y_RES]; - //uint16_t wz[DEPTH_X_RES*DEPTH_Y_RES]; - - // freenect_map_rgd_to_depth from registration.c - /* - uint32_t target_offset = dev->registration.reg_pad_info.start_lines * DEPTH_Y_RES; - 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; - wz[index] = depth_mm[index]; - // 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]; - } - */ - //freenect_apply_registration(®, data, wz); - //wz = output_mm; - - // then, convert x and y to world_x and world_y - //printf("Converting camera to world coordinates\n"); + freenect_map_rgb_to_depth(®, wz, data_ppm, data_ppm_reg); + write_PPM(PPMfile, data_ppm_reg); // see freenect_camera_to_world() in registration.c double ref_pix_size = reg.zero_plane_info.reference_pixel_size; @@ -392,10 +427,11 @@ void usage() { printf("Kinect Offline Registration\n"); printf("Usage:\n"); - printf(" kinect_register [-h] [-s | -a \n"); + printf(" kinect_register [-h] [-s | -a []\n"); printf(" -h: Display this help message\n" " -s: Save the registration parameters from a connected Kinect to \n" " -a: Apply the registration parameters from to (from 'record')\n" + " Optionally align a PPM file with the PGM file\n" ); exit(0); } @@ -403,7 +439,7 @@ void usage() int main(int argc, char **argv) { int c=1; - char *regfile=0, *PGMfile=0; + char *regfile=0, *PGMfile=0, *PPMfile=0; if (argc == 1) usage(); @@ -420,6 +456,10 @@ int main(int argc, char **argv) usage(); regfile = argv[c+1]; PGMfile = argv[c+2]; + if (argc == (c+4)) { + PPMfile = argv[c+3]; + printf("Will register color too...\n"); + } } c++; } @@ -427,7 +467,7 @@ int main(int argc, char **argv) if (regfile != 0 && PGMfile == 0) dump_registration(regfile); else - apply_registration(regfile, PGMfile); + apply_registration(regfile, PGMfile, PPMfile); return 0; } From d17657cf59bd58e51fcee31bca7dc570e11b743e Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Mon, 16 Apr 2012 00:03:12 +0200 Subject: [PATCH 06/11] write out RGB to XYZ file --- examples/kinect_register.c | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index 7f236393..cb58623c 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -371,8 +371,10 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) /* Convert DN to world */ // first, convert the DN to worldz - freenect_map_rgb_to_depth(®, wz, data_ppm, data_ppm_reg); - write_PPM(PPMfile, data_ppm_reg); + if (PPMfile != 0) { + freenect_map_rgb_to_depth(®, wz, data_ppm, data_ppm_reg); + write_PPM(PPMfile, data_ppm_reg); + } // see freenect_camera_to_world() in registration.c double ref_pix_size = reg.zero_plane_info.reference_pixel_size; @@ -414,7 +416,15 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) if (wz[i] == 0) continue; - fprintf(fp, "%d %d %d\n", (int16_t)wx[i], (int16_t)wy[i], wz[i]); + fprintf(fp, "%d %d %d", (int16_t)wx[i], (int16_t)wy[i], wz[i]); + if (PPMfile != 0) { + int ii = i*3; + fprintf( fp, " %d %d %d", + (uint8_t)data_ppm_reg[ii], + (uint8_t)data_ppm_reg[ii+1], + (uint8_t)data_ppm_reg[ii+2]); + } + fprintf(fp, "\n"); } fclose(fp); //printf("Wrote xyz file\n"); From 9a67ae841db4627e3b67d6121647dff6b0605296 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Mon, 16 Apr 2012 22:52:07 +0200 Subject: [PATCH 07/11] Removed unused code. Improved usage() --- examples/kinect_register.c | 86 ++++++++------------------------------ 1 file changed, 18 insertions(+), 68 deletions(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index cb58623c..16a3bffa 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -45,12 +45,6 @@ #define DEPTH_Y_RES 480 #define REG_X_VAL_SCALE 256 // "fixed-point" precision for double -> int32_t conversion -#define DEPTH_MIRROR_X 0 - - -#define S2D_PIXEL_CONST 10 -#define S2D_CONST_OFFSET 0.375 - void dump_registration(char* regfile) { @@ -60,7 +54,6 @@ void dump_registration(char* regfile) { if (freenect_init(&f_ctx, NULL) < 0) { printf("freenect_init() failed\n"); exit(0); - //return 1; } freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG); @@ -72,7 +65,6 @@ void dump_registration(char* regfile) { if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) { printf("Could not open device\n"); exit(0); - //return 1; } freenect_registration dev; @@ -121,51 +113,6 @@ void dump_registration(char* regfile) { } -// From: https://github.com/floe/libfreenect/blob/ee8666614e9ff620f49c83f466e5a99f6a0be4a0/src/registration.c -/* -int freenect_apply_registration(freenect_registration* reg, uint16_t* input_raw, uint16_t* output_mm) { - - memset(output_mm, DEPTH_NO_MM_VALUE, DEPTH_X_RES * DEPTH_Y_RES * sizeof(uint16_t)); // clear the output image - uint32_t target_offset = DEPTH_Y_RES * reg->reg_pad_info.start_lines; - uint32_t x,y,source_index = 0; - - for (y = 0; y < DEPTH_Y_RES; y++) { - for (x = 0; x < DEPTH_X_RES; x++) { - - // get the value at the current depth pixel, convert to millimeters - uint16_t metric_depth = reg->raw_to_mm_shift[ input_raw[source_index++] ]; - - // so long as the current pixel has a depth value - if (metric_depth == DEPTH_NO_MM_VALUE) continue; - if (metric_depth >= DEPTH_MAX_METRIC_VALUE) continue; - - // calculate the new x and y location for that pixel - // using registration_table for the basic rectification - // and depth_to_rgb_shift for determining the x shift - uint32_t reg_index = DEPTH_MIRROR_X ? ((y + 1) * DEPTH_X_RES - x - 1) : (y * DEPTH_X_RES + x); - uint32_t nx = (reg->registration_table[reg_index][0] + reg->depth_to_rgb_shift[metric_depth]) / REG_X_VAL_SCALE; - uint32_t ny = reg->registration_table[reg_index][1]; - - // ignore anything outside the image bounds - if (nx >= DEPTH_X_RES) continue; - - // convert nx, ny to an index in the depth image array - uint32_t target_index = (DEPTH_MIRROR_X ? ((ny + 1) * DEPTH_X_RES - nx - 1) : (ny * DEPTH_X_RES + nx)) - target_offset; - - // get the current value at the new location - uint16_t current_depth = output_mm[target_index]; - - // make sure the new location is empty, or the new value is closer - if ((current_depth == DEPTH_NO_MM_VALUE) || (current_depth > metric_depth)) { - output_mm[target_index] = metric_depth; // always save depth at current location - } - } - } - return 0; -} -*/ - - freenect_registration load_registration(char* regfile) { @@ -306,7 +253,9 @@ void write_xyz_bin(char *infile, double* x, double* y, uint16_t* z) fclose(fp); } -/// RGB -> depth mapping function (inverse of default FREENECT_DEPTH_REGISTERED mapping) + +// RGB -> depth mapping function (inverse of default FREENECT_DEPTH_REGISTERED mapping) +// From Florian void freenect_map_rgb_to_depth(freenect_registration* reg, uint16_t* depth_mm, uint8_t* rgb_raw, uint8_t* rgb_registered) { uint32_t target_offset = reg->reg_pad_info.start_lines * DEPTH_Y_RES; @@ -366,10 +315,6 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) } } write_PGM( PGMfile, wz, "d" ); - // inputfile.pgm -> inputfile.d.pgm - - /* Convert DN to world */ - // first, convert the DN to worldz if (PPMfile != 0) { freenect_map_rgb_to_depth(®, wz, data_ppm, data_ppm_reg); @@ -397,7 +342,6 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) write_xyz_bin(PGMfile, wx, wy, wz); /* Write out x,y,z ASCII file */ - //printf("Writing xyz file\n"); char *outfile_ascii = (char *)malloc(strlen(PGMfile) + 3); sprintf(outfile_ascii, "%s", PGMfile ); // append '.xyz' to input filename sprintf(outfile_ascii+strlen(PGMfile)-3, "%s", "xyz"); @@ -427,7 +371,6 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) fprintf(fp, "\n"); } fclose(fp); - //printf("Wrote xyz file\n"); } @@ -435,13 +378,20 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) void usage() { - printf("Kinect Offline Registration\n"); - printf("Usage:\n"); - printf(" kinect_register [-h] [-s | -a []\n"); - printf(" -h: Display this help message\n" - " -s: Save the registration parameters from a connected Kinect to \n" - " -a: Apply the registration parameters from to (from 'record')\n" - " Optionally align a PPM file with the PGM file\n" + printf("\nKinect Offline Registration\n"); + printf("\nUsage:\n"); + printf(" kinect_register [-h] -s | -a []\n"); + printf(" -h: Display this help message\n" + " -s: Save the registration parameters from a connected Kinect to \n" + " -a: Apply the registration parameters from to (from 'record')\n" + " Optionally align a PPM file with the PGM file\n" + "\n" + " Data Formats (units: mm):\n" + " file.x: 640x480 double of x values\n" + " file.y: 640x480 double of y values\n" + " file.z: 640x480 integer of z values\n" + " file.xyz: ASCII file of x y z r g b. RGB only if PPM argument provided\n" + " file.reg.ppm: PPM shifted so pixels align with file.{x,y,z} data\n\n" ); exit(0); } @@ -468,7 +418,7 @@ int main(int argc, char **argv) PGMfile = argv[c+2]; if (argc == (c+4)) { PPMfile = argv[c+3]; - printf("Will register color too...\n"); + //printf("Will register color too...\n"); } } c++; From 34a6345fc84c2fd23ca35c2a160664cbfa82d7dd Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Tue, 17 Apr 2012 19:58:52 +0200 Subject: [PATCH 08/11] Don't write raw data 2nd time '.d.pgm' --- examples/kinect_register.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index 16a3bffa..f86ed503 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -314,7 +314,7 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) wz[y * DEPTH_X_RES + x] = metric_depth < DEPTH_MAX_METRIC_VALUE ? metric_depth : DEPTH_MAX_METRIC_VALUE; } } - write_PGM( PGMfile, wz, "d" ); + // write_PGM( PGMfile, wz, "d" ); if (PPMfile != 0) { freenect_map_rgb_to_depth(®, wz, data_ppm, data_ppm_reg); From b9c4aeede77d3c5ce6eb89b21651f43f00922a14 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Fri, 24 Aug 2012 20:46:19 -0700 Subject: [PATCH 09/11] xyz ASCII file is now PLY format --- examples/kinect_register.c | 98 ++++++++++++++++++++++++++------------ 1 file changed, 67 insertions(+), 31 deletions(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index f86ed503..0737ffbc 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -254,6 +254,65 @@ void write_xyz_bin(char *infile, double* x, double* y, uint16_t* z) } +void write_PLY(char *outfile, double* x, double* y, uint16_t* z, uint8_t* rgb) +{ + FILE *fp; + int i; + fp = fopen(outfile, "w"); + if (!fp) { + perror(outfile); + exit(1); + } + + int nel=0; + for (i=0; i<(DEPTH_X_RES*DEPTH_Y_RES); i++) { + if (x[i] == 0 && y[i] == 0) + continue; + if (z[i] == 0) + continue; + nel++; + } + + fprintf(fp, "ply\n"); + fprintf(fp, "format ascii 1.0\n"); + fprintf(fp, "comment Generated by kinect_register program\n"); + fprintf(fp, "comment Author: Ken Mankoff\n"); + fprintf(fp, "comment DOI: \n"); + fprintf(fp, "element vertex %d\n", nel); + fprintf(fp, "property float x\n"); + fprintf(fp, "property float y\n"); + fprintf(fp, "property float z\n"); + if (rgb != 0) { + fprintf(fp, "property float r\n"); + fprintf(fp, "property float g\n"); + fprintf(fp, "property float b\n"); + } + //fprintf(fp, "property list uchar int vertex_index\n"); + fprintf(fp, "end_header\n"); + + for (i=0; i<(DEPTH_X_RES*DEPTH_Y_RES); i++) { + + if (x[i] == 0 && y[i] == 0) + continue; + + if (z[i] == 0) + continue; + + fprintf(fp, "%d %d %d", (int16_t)x[i], (int16_t)y[i], z[i]); + if (rgb != 0) { + int ii = i*3; + fprintf( fp, " %d %d %d", + (uint8_t)rgb[ii], + (uint8_t)rgb[ii+1], + (uint8_t)rgb[ii+2]); + } + fprintf(fp, "\n"); + nel++; + } + fclose(fp); + +} + // RGB -> depth mapping function (inverse of default FREENECT_DEPTH_REGISTERED mapping) // From Florian void freenect_map_rgb_to_depth(freenect_registration* reg, uint16_t* depth_mm, uint8_t* rgb_raw, uint8_t* rgb_registered) @@ -341,37 +400,14 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) /* Write out x,y,z binary files */ write_xyz_bin(PGMfile, wx, wy, wz); - /* Write out x,y,z ASCII file */ + /* Write out x,y,z (and maybe RGB) ASCII PLY file */ char *outfile_ascii = (char *)malloc(strlen(PGMfile) + 3); - sprintf(outfile_ascii, "%s", PGMfile ); // append '.xyz' to input filename - sprintf(outfile_ascii+strlen(PGMfile)-3, "%s", "xyz"); - FILE *fp; - fp = fopen(outfile_ascii, "w"); - if (!fp) { - perror(outfile_ascii); - exit(1); - } - - for (i=0; i<(DEPTH_X_RES*DEPTH_Y_RES); i++) { - - if (wx[i] == 0 && wy[i] == 0) - continue; - - if (wz[i] == 0) - continue; - - fprintf(fp, "%d %d %d", (int16_t)wx[i], (int16_t)wy[i], wz[i]); - if (PPMfile != 0) { - int ii = i*3; - fprintf( fp, " %d %d %d", - (uint8_t)data_ppm_reg[ii], - (uint8_t)data_ppm_reg[ii+1], - (uint8_t)data_ppm_reg[ii+2]); - } - fprintf(fp, "\n"); - } - fclose(fp); - + sprintf(outfile_ascii, "%s", PGMfile ); // append '.ply' to input filename + sprintf(outfile_ascii+strlen(PGMfile)-3, "%s", "ply"); + if (PPMfile != 0) + write_PLY(outfile_ascii, wx,wy,wz, data_ppm_reg); + else + write_PLY(outfile_ascii, wx,wy,wz, 0); } @@ -390,7 +426,7 @@ void usage() " file.x: 640x480 double of x values\n" " file.y: 640x480 double of y values\n" " file.z: 640x480 integer of z values\n" - " file.xyz: ASCII file of x y z r g b. RGB only if PPM argument provided\n" + " file.ply: ASCII PLY file of x y z r g b. RGB only if PPM argument provided\n" " file.reg.ppm: PPM shifted so pixels align with file.{x,y,z} data\n\n" ); exit(0); From cf0f510fed151a6fbeaa2d8d6989adcc865c9158 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Tue, 2 Oct 2012 08:12:10 +0200 Subject: [PATCH 10/11] DOI --- examples/kinect_register.c | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index 0737ffbc..8b96734f 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -414,20 +414,24 @@ void apply_registration(char* regfile, char* PGMfile, char* PPMfile) void usage() { - printf("\nKinect Offline Registration\n"); - printf("\nUsage:\n"); - printf(" kinect_register [-h] -s | -a []\n"); - printf(" -h: Display this help message\n" + printf("\n" + "Kinect Offline Registration\n" + "Citation: http://dx.doi.org/10.1002/esp.3332\n" + "\n" + "Usage:\n" + " kinect_register [-h] -s | -a []\n" + " -h: Display this help message\n" " -s: Save the registration parameters from a connected Kinect to \n" " -a: Apply the registration parameters from to (from 'record')\n" " Optionally align a PPM file with the PGM file\n" "\n" - " Data Formats (units: mm):\n" + "Data Formats (units: mm):\n" " file.x: 640x480 double of x values\n" " file.y: 640x480 double of y values\n" " file.z: 640x480 integer of z values\n" " file.ply: ASCII PLY file of x y z r g b. RGB only if PPM argument provided\n" - " file.reg.ppm: PPM shifted so pixels align with file.{x,y,z} data\n\n" + " file.reg.ppm: PPM shifted so pixels align with file.{x,y,z} data\n" + "\n" ); exit(0); } From 639dd17c651ff26b5f1e93f7c01e6930497c1730 Mon Sep 17 00:00:00 2001 From: "Kenneth D. Mankoff" Date: Tue, 2 Oct 2012 08:15:54 +0200 Subject: [PATCH 11/11] DOI in PLY --- examples/kinect_register.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/kinect_register.c b/examples/kinect_register.c index 8b96734f..93ec8c1b 100644 --- a/examples/kinect_register.c +++ b/examples/kinect_register.c @@ -277,7 +277,7 @@ void write_PLY(char *outfile, double* x, double* y, uint16_t* z, uint8_t* rgb) fprintf(fp, "format ascii 1.0\n"); fprintf(fp, "comment Generated by kinect_register program\n"); fprintf(fp, "comment Author: Ken Mankoff\n"); - fprintf(fp, "comment DOI: \n"); + fprintf(fp, "comment Citation: http://dx.doi.org/10.1002/esp.3332\n"); fprintf(fp, "element vertex %d\n", nel); fprintf(fp, "property float x\n"); fprintf(fp, "property float y\n");