double ReprojectError( double *R, double* Tc, v3_t Pts, v2_t Projpts, double * Kmatrix)
{
double error =0;
double b2[3];
double b_cam[3];
double b_proj[3];
double xij[2];
//K*[R|-Rtc]X//
//K*R*[X-tc] //
b2[0] = Pts.p[0] - Tc[0];
b2[1] = Pts.p[1] - Tc[1];
b2[2] = Pts.p[2] - Tc[2];
matrix_product331(R, b2, b_cam);
matrix_product331(Kmatrix, b_cam, b_proj);
xij[0] = -b_proj[0] / b_proj[2];
xij[1] = -b_proj[1] / b_proj[2];
double dx = Projpts.p[0] - xij[0];
double dy = Projpts.p[1] - xij[1];
//cout<< Projpts.p[0]<<" "<<Projpts.p[1]<<" "<< xij[0]<<" "<<xij[1]<<endl;
error += sqrt(dx * dx + dy * dy);
//cout<< Vx(Projpts[i])<<" "<<Vy(Projpts[i])<<" "<< xij[0]<<" "<<xij[1]<<" "<< sqrt(dx * dx + dy * dy) <<endl;
//}
return(error);
}
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];
}
}
void sfm_project2(camera_params_t *init, double f,
double *R, double *dt, double *b, double *p,
int explicit_camera_centers)
{
double *t;
double tnew[3];
double b_cam[3];
t = init->t;
tnew[0] = dt[0];
tnew[1] = dt[1];
tnew[2] = dt[2];
/* Project! */
if (!explicit_camera_centers) {
matrix_product331(R, 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_product(3, 3, 3, 1, R, b2, b_cam);
b_cam[0] = R[0] * b2[0] + R[1] * b2[1] + R[2] * b2[2];
b_cam[1] = R[3] * b2[0] + R[4] * b2[1] + R[5] * b2[2];
b_cam[2] = R[6] * b2[0] + R[7] * b2[1] + R[8] * b2[2];
}
if (!init->known_intrinsics) {
p[0] = -b_cam[0] * f / b_cam[2];
p[1] = -b_cam[1] * f / b_cam[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];
}
}
void sfm_project_rd(camera_params_t *init, double *K, double *k,
double *R, double *dt, double *b, double *p,
int undistort, int explicit_camera_centers)
{
double *t;
double tnew[3];
double b_cam[3];
t = init->t;
tnew[0] = dt[0];
tnew[1] = dt[1];
tnew[2] = dt[2];
/* Project! */
if (!explicit_camera_centers) {
matrix_product331(R, 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(R, b2, b_cam);
}
if (!init->known_intrinsics) {
p[0] = -b_cam[0] * K[0] / b_cam[2];
p[1] = -b_cam[1] * K[0] / b_cam[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];
}
// p[0] = b_cam[0] * K[0] / b_cam[2];
// p[1] = b_cam[1] * K[0] / b_cam[2];
/* Apply radial distortion */
if (undistort) {
#ifndef TEST_FOCAL
double k1 = k[0], k2 = k[1];
#else
double k1 = k[0] / init->k_scale;
double k2 = k[1] / init->k_scale;
#endif
double rsq = (p[0] * p[0] + p[1] * p[1]) / (K[0] * K[0]);
double factor = 1.0 + k1 * rsq + k2 * rsq * rsq;
p[0] *= factor;
p[1] *= factor;
}
}
v2_t sfm_project_final(camera_params_t *params, v3_t pt,
int explicit_camera_centers, int undistort)
{
double b_cam[3], b_proj[3];
double b[3] = { Vx(pt), Vy(pt), Vz(pt) };
v2_t proj;
/* Project! */
if (!explicit_camera_centers) {
matrix_product331(params->R, b, b_cam);
b_cam[0] += params->t[0];
b_cam[1] += params->t[1];
b_cam[2] += params->t[2];
} else {
double b2[3];
b2[0] = b[0] - params->t[0];
b2[1] = b[1] - params->t[1];
b2[2] = b[2] - params->t[2];
matrix_product331(params->R, b2, b_cam);
}
if (!params->known_intrinsics) {
b_proj[0] = b_cam[0] * params->f;
b_proj[1] = b_cam[1] * params->f;
b_proj[2] = b_cam[2];
b_proj[0] /= -b_proj[2];
b_proj[1] /= -b_proj[2];
if (undistort) {
double rsq =
(b_proj[0] * b_proj[0] + b_proj[1] * b_proj[1]) /
(params->f * params->f);
double factor = 1.0 + params->k[0] * rsq + params->k[1] * rsq * rsq;
b_proj[0] *= factor;
b_proj[1] *= factor;
}
} else {
/* Apply intrinsics */
double x_n = -b_cam[0] / b_cam[2];
double y_n = -b_cam[1] / b_cam[2];
double *k = params->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 = params->K_known;
b_proj[0] = K[0] * x_d + K[1] * y_d + K[2];
b_proj[1] = K[4] * y_d + K[5];
}
Vx(proj) = b_proj[0];
Vy(proj) = b_proj[1];
return proj;
}
/* Align two sets of points with a 3D rotation */
double align_3D_rotation(int n, v3_t *r_pts, v3_t *l_pts, double *R)
{
double A[9];
double U[9], S[3], V[9], VT[9], RT[9];
int i;
double error;
#if 0
if (n > 3) {
printf("A:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(r_pts[i]), Vy(r_pts[i]), Vz(r_pts[i]));
}
printf("B:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(l_pts[i]), Vy(l_pts[i]), Vz(l_pts[i]));
}
}
#endif
for (i = 0; i < 9; i++)
A[i] = 0.0;
for (i = 0; i < n; i++) {
double *a = l_pts[i].p, *b = r_pts[i].p;
// matrix_product(3, 1, 1, 3, l_pts[i].p, r_pts[i].p, tensor);
A[0] += a[0] * b[0];
A[1] += a[0] * b[1];
A[2] += a[0] * b[2];
A[3] += a[1] * b[0];
A[4] += a[1] * b[1];
A[5] += a[1] * b[2];
A[6] += a[2] * b[0];
A[7] += a[2] * b[1];
A[8] += a[2] * b[2];
}
// dgesvd_driver(3, 3, A, U, S, VT);
// printf("svd:\n");
// matrix_print(3, 3, A);
svd(3, 3, 1, 1, 1.0e-12, 1.0e-12, A, S, U, V, VT);
// printf("U:\n");
// matrix_print(3, 3, U);
// printf("VT:\n");
// matrix_print(3, 3, VT);
// printf("S:\n");
// matrix_print(3, 3, S);
matrix_product33(U, VT, RT);
matrix_transpose(3, 3, RT, R);
// printf("R:\n");
// matrix_print(3, 3, R);
if (matrix_determinant3(R) < 0.0) {
/* We're dealing with a reflection */
double tmp[9];
double reflectZ[9] = { 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0 };
matrix_product33(U, reflectZ, tmp);
matrix_product33(tmp, VT, RT);
matrix_transpose(3, 3, RT, R);
}
/* Compute error */
error = 0.0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product331(R, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
// printf("d[%d] = %0.6f\n", i, dist);
error += dist;
}
return error / n;
}
