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 }