void sfm_project(camera_params_t *init, double *K,
double *w, double *dt, double *b, double *p,
int explicit_camera_centers)
{
double *R, *t;
double Rnew[9];
double tnew[3];
double b_cam[3], b_proj[3];
R = init->R;
t = init->t;
rot_update(R, w, Rnew);
tnew[0] = dt[0]; // t[0] + dt[0];
tnew[1] = dt[1]; // t[1] + dt[1]; // 0.0
tnew[2] = dt[2]; // t[2] + dt[2]; // 0.0
/* Project! */
if (!explicit_camera_centers) {
matrix_product331(Rnew, b, b_cam);
b_cam[0] += tnew[0];
b_cam[1] += tnew[1];
b_cam[2] += tnew[2];
} else {
double b2[3];
b2[0] = b[0] - tnew[0];
b2[1] = b[1] - tnew[1];
b2[2] = b[2] - tnew[2];
matrix_product331(Rnew, b2, b_cam);
}
if (!init->known_intrinsics) {
matrix_product331(K, b_cam, b_proj);
p[0] = -b_proj[0] / b_proj[2];
p[1] = -b_proj[1] / b_proj[2];
} else {
/* Apply intrinsics */
double x_n = -b_cam[0] / b_cam[2];
double y_n = -b_cam[1] / b_cam[2];
double *k = init->k_known;
double rsq = x_n * x_n + y_n * y_n;
double factor = 1.0 + k[0] * rsq +
k[1] * rsq * rsq + k[4] * rsq * rsq * rsq;
double dx_x = 2 * k[2] * x_n * y_n + k[3] * (rsq + 2 * x_n * x_n);
double dx_y = k[2] * (rsq + 2 * y_n * y_n) + 2 * k[3] * x_n * y_n;
double x_d = x_n * factor + dx_x;
double y_d = y_n * factor + dx_y;
double *K = init->K_known;
p[0] = K[0] * x_d + K[1] * y_d + K[2];
p[1] = K[4] * y_d + K[5];
}
}
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_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);
}
}
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
}
/* Refine the position of a single camera */
void camera_refine(int num_points, v3_t *points, v2_t *projs,
camera_params_t *params, int adjust_focal,
int estimate_distortion)
{
if (adjust_focal) {
int num_camera_params = 7;
double x[9] = { params->t[0], params->t[1], params->t[2],
0.0, 0.0, 0.0, params->f, params->k[0], params->k[1] };
double Rnew[9];
sfm_global_t globs;
int focal_constraint = 0;
if (estimate_distortion)
num_camera_params += 2;
globs.num_cameras = 1;
globs.num_points = num_points;
globs.est_focal_length = 1;
globs.const_focal_length = 0;
globs.explicit_camera_centers = 1;
globs.global_params.f = params->f;
globs.init_params = params;
globs.estimate_distortion = estimate_distortion;
global_num_points = num_points;
global_params = &globs;
global_points = points;
global_projections = projs;
global_round = 0;
if (params->constrained[6]) {
printf("[camera_refine] Constraining focal length to %0.3f "
"(weight: %0.3f)\n",
params->constraints[6],
num_points * params->weights[6]);
focal_constraint = 1;
global_init_focal = params->constraints[6];
global_constrain_focal = 1;
global_constrain_focal_weight =
1.0e0 /*1.0e1*/ * num_points * params->weights[6];
} else {
focal_constraint = 0;
global_init_focal = 0.0;
global_constrain_focal = 0;
global_constrain_focal_weight = 0.0;
}
if (estimate_distortion) {
global_constrain_rd_weight = 0.05 * num_points;
// 1.0e-1 * num_points;
}
lmdif_driver2(camera_refine_residual,
2 * num_points + focal_constraint +
2 * estimate_distortion,
num_camera_params, x, 1.0e-12);
/* Copy out the parameters */
memcpy(params->t, x + 0, 3 * sizeof(double));
rot_update(params->R, x + 3, Rnew);
memcpy(params->R, Rnew, 9 * sizeof(double));
params->f = x[6];
if (estimate_distortion) {
params->k[0] = x[7];
params->k[1] = x[8];
}
} else {
double x[6] = { params->t[0], params->t[1], params->t[2],
0.0, 0.0, 0.0 };
double Rnew[9];
sfm_global_t globs;
globs.num_cameras = 1;
globs.num_points = num_points;
globs.est_focal_length = 0;
globs.const_focal_length = 1;
globs.explicit_camera_centers = 1;
globs.global_params.f = params->f;
globs.init_params = params;
globs.estimate_distortion = estimate_distortion;
global_num_points = num_points;
global_params = &globs;
global_points = points;
global_projections = projs;
global_round = 0;
global_init_focal = 0.0;
global_constrain_focal = 0;
global_constrain_focal_weight = 0.0;
lmdif_driver2(camera_refine_residual, 2 * num_points, 6, x, 1.0e-12);
/* Copy out the parameters */
memcpy(params->t, x + 0, 3 * sizeof(double));
rot_update(params->R, x + 3, Rnew);
memcpy(params->R, Rnew, 9 * sizeof(double));
}
}
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]);
}
}
