static void sfm_mot_project_point(int j, int i, 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[3] = { 0.0, 0.0, 0.0 }, dt[3] = { 0.0, 0.0, 0.0 };
/* Compute intrinsics */
K[0] = K[4] = globs->init_params[j].f; // globs->global_params.f;
if (1 || j > 0) {
sfm_project(globs->init_params + j, K, w, dt, bi, xij,
globs->explicit_camera_centers);
} else {
/* Keep first camera at origin */
double w0[3] = { 0.0, 0.0, 0.0 };
double dt0[3] = { 0.0, 0.0, 0.0 };
sfm_project(globs->init_params + j, K, w0, dt0, bi, xij,
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);
}
}
void run_sfm(int num_pts, int num_cameras, int ncons,
char *vmask,
double *projections,
int est_focal_length,
int const_focal_length,
int undistort,
int explicit_camera_centers,
camera_params_t *init_camera_params,
v3_t *init_pts,
int use_constraints,
int use_point_constraints,
v3_t *pt_constraints,
double pt_constraint_weight,
int fix_points,
int optimize_for_fisheye,
double eps2,
double *Vout,
double *Sout,
double *Uout, double *Wout
/* size num_cameras ** 2 * cnp * cnp */)
{
int cnp;
double *params;
#ifdef SBA_V121
double opts[6]; // opts[5];
#else
double opts[3];
#endif
double info[10];
int i, j, idx, base;
int num_camera_params, num_pt_params, num_params;
sfm_global_t global_params;
// #ifndef SBA_V121
camera_constraints_t *constraints = NULL;
// #endif
point_constraints_t *point_constraints = NULL;
const double f_scale = 0.001;
const double k_scale = 5.0;
if (est_focal_length)
cnp = 7;
else
cnp = 6;
if (undistort)
cnp += 2;
num_camera_params = cnp * num_cameras;
num_pt_params = 3 * num_pts;
num_params = num_camera_params + num_pt_params;
params = (double *) safe_malloc(sizeof(double) * num_params, "params");
/* Fill parameters */
for (j = 0; j < num_cameras; j++) {
int c = 0;
#ifdef TEST_FOCAL
init_camera_params[j].f_scale = f_scale;
init_camera_params[j].k_scale = k_scale;
#else
init_camera_params[j].f_scale = 1.0;
init_camera_params[j].k_scale = 1.0;
#endif
/* Translation is zero */
params[cnp * j + 0] = init_camera_params[j].t[0]; // 0.0;
params[cnp * j + 1] = init_camera_params[j].t[1]; // 0.0;
params[cnp * j + 2] = init_camera_params[j].t[2]; // 0.0;
/* Rotation is zero */
params[cnp * j + 3] = 0.0;
params[cnp * j + 4] = 0.0;
params[cnp * j + 5] = 0.0;
if (est_focal_length) {
/* Focal length is initial estimate */
#ifndef TEST_FOCAL
params[cnp * j + 6] = init_camera_params[j].f;
#else
params[cnp * j + 6] =
init_camera_params[j].f * init_camera_params[j].f_scale;
#endif
c = 7;
} else {
c = 6;
}
if (undistort) {
#ifndef TEST_FOCAL
params[cnp * j + c] = init_camera_params[j].k[0];
params[cnp * j + c+1] = init_camera_params[j].k[1];
#else
double scale = init_camera_params[j].k_scale;
params[cnp * j + c] = init_camera_params[j].k[0] * scale;
params[cnp * j + c+1] = init_camera_params[j].k[1] * scale;
#endif
}
}
base = num_camera_params;
for (i = 0; i < num_pts; i++) {
params[base + 3 * i + 0] = Vx(init_pts[i]);
params[base + 3 * i + 1] = Vy(init_pts[i]);
params[base + 3 * i + 2] = Vz(init_pts[i]);
}
opts[0] = 1.0e-3;
opts[1] = 1.0e-10; // 1.0e-15;
opts[2] = eps2; // 0.0; // 1.0e-10; // 1.0e-15;
#ifdef SBA_V121
opts[3] = 1.0e-12;
// opts[4] = 4.0e-2;
opts[4] = 0.0;
opts[5] = 4.0e-2; // change this back to opts[4] for sba v1.2.1
#endif
// opts[1] = 1.0e-8;
// opts[2] = 1.0e-8;
// #ifndef SBA_V121
/* Create the constraints */
if (use_constraints) {
constraints =
(camera_constraints_t *)
malloc(num_cameras * sizeof(camera_constraints_t));
for (i = 0; i < num_cameras; i++) {
constraints[i].constrained = (char *) malloc(cnp);
constraints[i].constraints =
(double *) malloc(sizeof(double) * cnp);
constraints[i].weights = (double *) malloc(sizeof(double) * cnp);
memcpy(constraints[i].constrained,
init_camera_params[i].constrained, cnp * sizeof(char));
memcpy(constraints[i].constraints,
init_camera_params[i].constraints, cnp * sizeof(double));
memcpy(constraints[i].weights,
init_camera_params[i].weights, cnp * sizeof(double));
#ifdef TEST_FOCAL
if (est_focal_length) {
constraints[i].constraints[6] *= f_scale;
constraints[i].weights[6] *= (1.0 / (f_scale * f_scale));
}
if (undistort) {
constraints[i].constraints[7] *= k_scale;
constraints[i].weights[7] *= (1.0 / (k_scale * k_scale));
constraints[i].constraints[8] *= k_scale;
constraints[i].weights[8] *= (1.0 / (k_scale * k_scale));
}
#endif
}
}
// #endif
if (use_point_constraints) {
point_constraints =
(point_constraints_t *)
malloc(num_pts * sizeof(point_constraints_t));
for (i = 0; i < num_pts; i++) {
if (Vx(pt_constraints[i]) == 0.0 &&
Vy(pt_constraints[i]) == 0.0 &&
Vz(pt_constraints[i]) == 0.0) {
point_constraints[i].constrained = 0;
point_constraints[i].constraints[0] = 0.0;
point_constraints[i].constraints[1] = 0.0;
point_constraints[i].constraints[2] = 0.0;
point_constraints[i].weight = 0.0;
} else {
// printf("[run_sfm] Constraining point %d\n", i);
point_constraints[i].constrained = 1;
point_constraints[i].weight = pt_constraint_weight;
point_constraints[i].constraints[0] = Vx(pt_constraints[i]);
point_constraints[i].constraints[1] = Vy(pt_constraints[i]);
point_constraints[i].constraints[2] = Vz(pt_constraints[i]);
}
}
}
/* Fill global param struct */
global_params.num_cameras = num_cameras;
global_params.num_points = num_pts;
global_params.num_params_per_camera = cnp;
global_params.est_focal_length = est_focal_length;
global_params.const_focal_length = const_focal_length;
global_params.estimate_distortion = undistort;
global_params.explicit_camera_centers = explicit_camera_centers,
global_params.global_params.f = 1.0;
global_params.init_params = init_camera_params;
global_last_ws =
safe_malloc(3 * num_cameras * sizeof(double), "global_last_ws");
global_last_Rs =
safe_malloc(9 * num_cameras * sizeof(double), "global_last_ws");
global_params.points = init_pts;
for (i = 0; i < num_cameras; i++) {
global_last_ws[3 * i + 0] = 0.0;
global_last_ws[3 * i + 1] = 0.0;
global_last_ws[3 * i + 2] = 0.0;
memcpy(global_last_Rs + 9 * i,
init_camera_params[i].R, 9 * sizeof(double));
}
/* Run sparse bundle adjustment */
#define MAX_ITERS 150 // 256
#define VERBOSITY 3
#ifdef SBA_V121
if (fix_points == 0) {
if (optimize_for_fisheye == 0) {
sba_motstr_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, 3, projections, NULL, 2,
//remove NULL in prev line for sba v1.2.1
sfm_project_point3, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints,
use_point_constraints,
point_constraints, Vout, Sout, Uout, Wout);
} else {
sba_motstr_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, 3, projections, NULL, 2,
sfm_project_point2_fisheye, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints,
use_point_constraints,
point_constraints, Vout, Sout, Uout, Wout);
}
} else {
if (optimize_for_fisheye == 0) {
sba_mot_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, projections, NULL, 2,
sfm_project_point3_mot, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints);
} else {
sba_mot_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, projections, NULL, 2,
sfm_project_point2_fisheye_mot, NULL,
(void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints);
}
}
#else
if (fix_points == 0) {
sba_motstr_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, 3, projections, 2,
sfm_project_point2, NULL, (void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info,
use_constraints, constraints,
Vout, Sout, Uout, Wout);
} else {
sba_mot_levmar(num_pts, num_cameras, ncons,
vmask, params, cnp, projections, 2,
sfm_mot_project_point, NULL, (void *) (&global_params),
MAX_ITERS, VERBOSITY, opts, info);
}
#endif
printf("[run_sfm] Number of iterations: %d\n", (int) info[5]);
printf("info[6] = %0.3f\n", info[6]);
/* Copy out the params */
for (j = 0; j < num_cameras; j++) {
double *dt = params + cnp * j + 0;
double *w = params + cnp * j + 3;
double Rnew[9];
int c;
/* Translation */
init_camera_params[j].t[0] = dt[0];
init_camera_params[j].t[1] = dt[1];
init_camera_params[j].t[2] = dt[2];
// init_camera_params[j].t[0] += dt[0];
// init_camera_params[j].t[1] += dt[1];
// init_camera_params[j].t[2] += dt[2];
/* Rotation */
rot_update(init_camera_params[j].R, w, Rnew);
memcpy(init_camera_params[j].R, Rnew, 9 * sizeof(double));
/* Focal length */
if (est_focal_length) {
c = 7;
#ifndef TEST_FOCAL
init_camera_params[j].f = params[cnp * j + 6];
#else
init_camera_params[j].f =
params[cnp * j + 6] / init_camera_params[j].f_scale;
#endif
} else {
c = 6;
}
if (undistort) {
#ifndef TEST_FOCAL
init_camera_params[j].k[0] = params[cnp * j + c];
init_camera_params[j].k[1] = params[cnp * j + c+1];
#else
double scale = init_camera_params[j].k_scale;
init_camera_params[j].k[0] = params[cnp * j + c] / scale;
init_camera_params[j].k[1] = params[cnp * j + c+1] / scale;
#endif
}
#ifdef TEST_FOCAL
init_camera_params[j].f_scale = 1.0;
init_camera_params[j].k_scale = 1.0;
#endif
}
base = num_camera_params;
for (i = 0; i < num_pts; i++) {
Vx(init_pts[i]) = params[base + 3 * i + 0];
Vy(init_pts[i]) = params[base + 3 * i + 1];
Vz(init_pts[i]) = params[base + 3 * i + 2];
}
// #define DEBUG_SFM
#ifdef DEBUG_SFM
for (i = 0; i < num_cameras; i++) {
int num_projs = 0;
double error = 0.0;
double error_max = 0.0;
int idx_max = 0;
double px_max = 0.0, py_max = 0.0;
double K[9] = { init_camera_params[i].f, 0.0, 0.0,
0.0, init_camera_params[i].f, 0.0,
0.0, 0.0, 1.0 };
double w[3] = { 0.0, 0.0, 0.0 };
double dt[3] = { init_camera_params[i].t[0],
init_camera_params[i].t[1],
init_camera_params[i].t[2] };
// double dt[3] = { 0.0, 0.0, 0.0 };
for (j = 0; j < num_pts; j++) {
double b[3], pr[2];
double dx, dy, dist;
if (!vmask[j * num_cameras + i])
continue;
b[0] = Vx(init_pts[j]);
b[1] = Vy(init_pts[j]);
b[2] = Vz(init_pts[j]);
sfm_project(&(init_camera_params[i]), K, w, dt, b, pr,
global_params.explicit_camera_centers);
dx = pr[0] - Vx(projections[j * num_cameras + i]);
dy = pr[1] - Vy(projections[j * num_cameras + i]);
dist = dx * dx + dy * dy;
error += dist;
if (dist > error_max) {
idx_max = j;
error_max = dist;
px_max = Vx(projections[j * num_cameras + i]);
py_max = Vy(projections[j * num_cameras + i]);
}
num_projs++;
}
printf("Camera %d: error = %0.3f (%0.3f)\n", i,
error, sqrt(error / num_projs));
printf(" error_max = %0.3f (%d)\n", sqrt(error_max), idx_max);
printf(" proj = %0.3f, %0.3f\n", px_max, py_max);
}
#endif /* DEBUG_SFM */
free(params);
// #ifndef SBA_V121
if (use_constraints) {
for (i = 0; i < num_cameras; i++) {
free(constraints[i].constraints);
free(constraints[i].constrained);
free(constraints[i].weights);
}
free(constraints);
}
free(global_last_ws);
free(global_last_Rs);
// #endif
}
