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);
}
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]);
}
}