static void sfm_project_point2_fisheye(int j, int i, double *aj, double *bi, double *xij, void *adata) { sfm_global_t *globs = (sfm_global_t *)adata; double f; double *w, *dt; double xij_tmp[2]; /* Compute intrinsics */ if (!globs->est_focal_length) { f = globs->init_params[j].f; // globs->global_params.f; } else if (globs->const_focal_length) { printf("[sfm_project_point2_fisheye] ERROR: case of constant focal length " "has not been implemented.\n"); f = globs->global_params.f; } else { #ifndef TEST_FOCAL f = aj[6]; #else f = aj[6] / globs->init_params[j].f_scale; #endif } /* Compute translation, rotation update */ dt = aj + 0; w = aj + 3; #ifdef COLIN_HACK w[0] = w[1] = w[2] = 0.0; dt[2] = 0.0; #endif if (w[0] != global_last_ws[3 * j + 0] || w[1] != global_last_ws[3 * j + 1] || w[2] != global_last_ws[3 * j + 2]) { // printf("updating w: %0.3f, %0.3f, %0.3f\n", w[0], w[1], w[2]); rot_update(globs->init_params[j].R, w, global_last_Rs + 9 * j); global_last_ws[3 * j + 0] = w[0]; global_last_ws[3 * j + 1] = w[1]; global_last_ws[3 * j + 2] = w[2]; } sfm_project2(globs->init_params + j, f, global_last_Rs + 9 * j, dt, bi, xij_tmp, globs->explicit_camera_centers); /* Distort the point */ sfm_fisheye_distort(globs->init_params + j, xij_tmp, xij); }
//ALEX: The existing fisheye projection does not account for radial distortion static void sfm_project_fisheye_point3(int j, int i, double *aj, double *bi, double *xij, void *adata) { sfm_global_t *globs = (sfm_global_t *) adata; double K[9] = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double *w, *dt, *k; /* Compute intrinsics */ if (!globs->est_focal_length) { K[0] = K[4] = globs->init_params[j].f; // globs->global_params.f; } else if (globs->const_focal_length) { printf("Error: case of constant focal length " "has not been implemented.\n"); K[0] = K[4] = globs->global_params.f; } else { #ifndef TEST_FOCAL K[0] = K[4] = aj[6]; #else K[0] = K[4] = aj[6] / globs->init_params[j].f_scale; #endif } /* Compute translation, rotation update */ dt = aj + 0; w = aj + 3; if (globs->est_focal_length) k = aj + 7; else k = aj + 6; if (w[0] != global_last_ws[3 * j + 0] || w[1] != global_last_ws[3 * j + 1] || w[2] != global_last_ws[3 * j + 2]) { rot_update(globs->init_params[j].R, w, global_last_Rs + 9 * j); global_last_ws[3 * j + 0] = w[0]; global_last_ws[3 * j + 1] = w[1]; global_last_ws[3 * j + 2] = w[2]; } sfm_project_rd(globs->init_params + j, K, k, global_last_Rs + 9 * j, dt, bi, xij, globs->estimate_distortion, globs->explicit_camera_centers); double FocalLength = (globs->init_params + j)->f_focal; if ( globs->estimate_fisheye_focal ) { #ifndef TEST_FOCAL FocalLength = aj[ globs->ff_offset ]; #else FocalLength = aj[ globs->ff_offset ]/ globs->init_params[j].fisheye_scale; #endif } if (( FocalLength > (globs->init_params + j)->f_focal * 1.5 ) || ( FocalLength < (globs->init_params + j)->f_focal * 0.6666 )) { printf("**********ALEX:BUGCHECK focal length = %f**********\n", FocalLength ); } sfm_fisheye_distort(globs->init_params + j, FocalLength, xij, xij); }
static void sfm_project_point2_fisheye(int j, int i, double *aj, double *bi, double *xij, void *adata) { sfm_global_t *globs = (sfm_global_t *) adata; double f; double *w, *dt; double xij_tmp[2]; /* Compute intrinsics */ if (!globs->est_focal_length) { f = globs->init_params[j].f; // globs->global_params.f; } else if (globs->const_focal_length) { printf("Error: case of constant focal length " "has not been implemented.\n"); f = globs->global_params.f; } else { #ifndef TEST_FOCAL f = aj[6]; #else f = aj[6] / globs->init_params[j].f_scale; #endif } /* Compute translation, rotation update */ dt = aj + 0; w = aj + 3; #ifdef COLIN_HACK w[0] = w[1] = w[2] = 0.0; dt[2] = 0.0; #endif if (w[0] != global_last_ws[3 * j + 0] || w[1] != global_last_ws[3 * j + 1] || w[2] != global_last_ws[3 * j + 2]) { // printf("updating w: %0.3f, %0.3f, %0.3f\n", w[0], w[1], w[2]); rot_update(globs->init_params[j].R, w, global_last_Rs + 9 * j); global_last_ws[3 * j + 0] = w[0]; global_last_ws[3 * j + 1] = w[1]; global_last_ws[3 * j + 2] = w[2]; } sfm_project2(globs->init_params + j, f, global_last_Rs + 9 * j, dt, bi, xij_tmp, globs->explicit_camera_centers); /* Distort the point */ //ALEX: Added a focal length parameter double FocalLength = (globs->init_params + j)->f_focal; if ( globs->estimate_fisheye_focal ) { #ifndef TEST_FOCAL FocalLength = aj[ globs->ff_offset ]; #else FocalLength = aj[ globs->ff_offset ]/ globs->init_params[j].fisheye_scale; #endif FocalLength = aj[ globs->ff_offset ]; } if (( FocalLength > (globs->init_params + j)->f_focal * 1.5 ) || ( FocalLength < (globs->init_params + j)->f_focal * 0.6666 )) { printf("**********ALEX:BUGCHECK focal length = %f**********\n", FocalLength ); } sfm_fisheye_distort(globs->init_params + j, FocalLength, xij_tmp, xij); static int sample = 1; sample++; if (( sample % 100 ) == 0 ) { //printf("incoming = %f %f %f\n", (float)bi[0], (float)bi[1], (float)bi[2]); //printf("projected = %f %f \n", (float)xij_tmp[0], (float)xij_tmp[1]); //printf("focal = %f\n",f); //printf("FD = %f %f \n", (float)xij[0], (float)xij[1]); } }