static void sfm_project_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);
}
static void sfm_project_point(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;
/* 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
{
K[0] = K[4] = aj[6];
}
/* 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 (globs->estimate_distortion == 0) {
sfm_project(globs->init_params + j, K, w, dt, bi, xij,
globs->explicit_camera_centers);
} else {
double Rnew[9];
rot_update(globs->init_params->R, w, Rnew);
sfm_project_rd(globs->init_params + j, K, aj + 7,
Rnew, dt, bi, xij, 1, globs->explicit_camera_centers);
}
}
