/* Last edited on 2022-01-01 00:18:17 by stolfi */ if (o->writeStitched) { image_stitch_write_stitched(o->outPrefix, nc, C1, &(M.dir), C2, &(M.inv)); /* Find the image domain: */ interval_t B[2]; /* Bounding box of mappe corners. */ box_empty(2, B); for (int32_t kc = 0; kc < 4; kc++) { for (int32_t j = 0; j < 2; j++) { B[j] = interval_include(&(B[j]), C1[kc].c[j]); B[j] = interval_include(&(B[j]), C2[kc].c[j]); } } fprintf(stderr, "final image range:\n"); for (int32_t j = 0; j < 2; j++) { fprintf(stderr, "axis %d : ", j); interval_gen_print(stderr, &(B[j]), "%12.6f", "( ", " ", " )\n"); } } hr2_pmap_t image_stitch_optimize_pmap ( hr2_pmap_t *M0, r2_t *size1, r2_vec_t *p1, r2_t *size2, r2_vec_t *p2, bool_t verbose ) { bool_t debug = TRUE; int np = p1->ne; assert(np == p2->ne); if (np < 4) { fprintf(stderr, "too few point pairs (%d) for projective map, using affine map\n", np); return (*M0); } /* Choose scale factors for the pack/unpack routines: */ r3x3_t G; image_stitch_get_matrix_scale(&(M0->dir), p1, p2, &G); double maxErr = 0.5; /* Max root-mean-square position error. */ int nx = 8; /* Number of packed parameters. */ auto void pack_parameters(hr2_pmap_t *M, int nx, double x[]); /* Packs the homogeneous projective matrix {M} to the parameter vector {x[0..nx-1]} to. Implicitly normalizes the matrix will have {M[0,0] == 1}. Requires {nx==8}. */ auto void unpack_parameters(int nx, double x[], hr2_pmap_t *M); /* Unpacks the parameter vector {x[0..nx-1]} to the homogeneous projective matrix {M}. The matrix will have {M[0,0] == 1}. Requires {nx==8}. */ auto double goalf(int nx, double x[]); /* Computes the mean squared error in the positions of the mapped points, given the packed parameters {x[0..nx-1]}. */ auto bool_t is_ok(int nx, double x[], double Fx); /* Returns true if the squared error {Fx} is small enough. */ auto void plot_goalf(FILE *pfile, int nx, double x[], double r); /* Prints to {pfile} the values {goalf(nx,x+e*u)} for some random direction {u} in {\RR^{nx}} and for {e} varying from {-r} to {+r}. */ double esq = image_stitch_mean_err_sqr(p1, &(M0->dir), p2, &(M0->inv)); double dMax = sqrt(esq); /* Search radius around initial guess. */ double rIni = 0.250*dMax; /* Initial probe radius. */ double rMin = 0.5; /* Minimum probe radius. */ double rMax = 0.5*dMax; /* Maximum probe radius. */ int ns = (nx+1)*(nx+2)/2; /* Number of sampling points. */ int maxIters = 3; /* Max number of major iterations. */ int maxEvals = maxIters*(ns+1); /* Max number of goal evaluations. */ if (debug) { fprintf(stderr, "sensitivity matrix =\n"); r3x3_gen_print(stderr, &G, "%13.6e", "\n", "\n", "\n", " [ ", " ", " ]"); fprintf(stderr, "initial mean error squared = %22.16e\n", esq); fprintf(stderr, "estimated distance from optimum = %13.6f\n", dMax); fprintf(stderr, "probe radius = %13.6f [ %13.6f _ %13.6f ]\n", rIni, rMin, rMax); } double x[nx]; /* Initial guess and final optimum parameters. */ pack_parameters(M0, nx, x); if (verbose) { FILE *pfile = open_write("out/plot.txt", TRUE); plot_goalf(pfile, nx, x, rIni); fclose(pfile); } sve_minn_iterate(nx, &goalf, &is_ok, x, dMax, rIni, rMin, rMax, maxEvals, debug); hr2_pmap_t M; /* Optimized matrix. */ unpack_parameters(nx, x, &M); return M; /* --- internal procs ----------------------------------------------------- */ void pack_parameters(hr2_pmap_t *M, int nx, double x[]) { assert(nx == 8); int i, j; int k = 0; double w = M->dir.c[0][0]; assert(w > 1.0e-15); for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { if ((i == 0) && (j == 0)) { assert(M->dir.c[i][j] == w); } else if (i == 0) { x[k] = M->dir.c[i][j]/w; k++; } /* Try to equalize the scales: */ x[k] *= G.c[i][j]; } } } void unpack_parameters(int nx, double x[], hr2_pmap_t *M) { assert(nx == 8); int i, j; int k = 0; for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { if ((i == 0) && (j == 0)) { M->dir.c[i][j] = 1.0; } else { M->dir.c[i][j] = x[k]; k++; } /* Try to equalize the scales: */ M->dir.c[i][j] /= G.c[i][j]; } } r3x3_inv(&(M->dir), &(M->inv)); } double goalf(int nx, double x[]) { hr2_pmap_t M; unpack_parameters(nx, x, &M); if (debug) { r3x3_gen_print(stderr, &(M.dir), "%13.6e", "\n", "\n", "\n", " [ ", " ", " ]"); } /* Mean square error on point lists: */ double esq = image_stitch_mean_err_sqr(p1, &(M.dir), p2, &(M.inv)); /* Deformation penalty: */ double dsq1 = image_stitch_deform_sqr(size1, &(M.dir)); double dsq2 = image_stitch_deform_sqr(size2, &(M.inv)); if (debug) { fprintf(stderr, " average squared error = %13.6e", esq); fprintf(stderr, " deformation squared = %13.6e %13.6e\n", dsq1, dsq2); } return esq + dsq1 + dsq2; } bool_t is_ok(int nx, double x[], double Fx) { return Fx < maxErr*maxErr; } void plot_goalf(FILE *pfile, int nx, double x[], double r) { double y[nx]; int N = 20; fprintf(pfile, "# nx = %d\n", nx); int k; for (k = -N; k <= N; k++) { double e = r*((double)k)/((double)N); fprintf(pfile, "%+4d %14.6e ", k, e); rn_copy(nx, x, y); int i; for (i = 0; i < nx; i++) { double sv = y[i]; y[i] = x[i] + e; double fy = goalf(nx, y); fprintf(pfile, " %14.6e", fy); y[i] = sv; } fprintf(pfile, "\n"); } } } void image_stitch_get_matrix_scale(r3x3_t *M, r2_vec_t *p1, r2_vec_t *p2, r3x3_t *G); /* Computes a suitable scale factor {G[i][j]} for each element of {M}, so that an infinitesimal change of {eps} in element {M[i][j]} causes a root-men-square change of {eps*G[i][j]} in the length of the vectors {d[k] = p1[k]*M - p2[k]*M^{-1}}. Since {M[0][0]} is assumed to be always 1, {G[0][0]} is meningless and is arbitrarily set to 1. */ void image_stitch_get_matrix_scale(r3x3_t *M, r2_vec_t *p1, r2_vec_t *p2, r3x3_t *G) { int np = p1->ne; assert(np == p2->ne); /* Compute the inverse matrix {N} of {M}: */ assert(M->c[0][0] == 1); r3x3_t N; r3x3_inv(M, &N); int i, j; for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { if ((i == 0) && (j == 0)) { G->c[i][j] = 1.0; } else { /* Determine the magnitude {mag} for the element {M[i][j]}: */ double mag; if (i == 0) { mag = fmax(fabs(M->c[0][1]), fabs(M->c[0][2])); } else if (j == 0) { mag = 1.0; } else { mag = fmax ( fmax(fabs(M->c[1][1]), fabs(M->c[1][2])), fmax(fabs(M->c[2][1]), fabs(M->c[2][2])) ); } /* Compute the perturbed matrix {M1} and its inverse {N1}: */ r3x3_t M1 = (*M); double eps = hypot(M1.c[i][j], 1.0e-6*mag); M1.c[i][j] += eps; r3x3_t N1; r3x3_inv(&M1, &N1); /* Map the points and note the rms displacements: */ int k; double sum2 = 0; for (k = 0; k < np; k++) { r2_t *p1k = &(p1->e[k]); r2_t q1k; image_stitch_map_point(p1k, M, &q1k); r2_t r1k; image_stitch_map_point(p1k, &M1, &r1k); r2_t *p2k = &(p2->e[k]); r2_t q2k; image_stitch_map_point(p2k, &N, &q2k); r2_t r2k; image_stitch_map_point(p2k, &N1, &r2k); /* Mismatch of original and perturbed matrices: */ r2_t dq; r2_sub(&q1k, &q2k, &dq); r2_t dr; r2_sub(&r1k, &r2k, &dr); /* Change in mismatch: */ double d2 = r2_dist_sqr(&dq, &dr); sum2 += d2; } /* Compute the derivative {drdM} of displacement wrt {M[i][j]}: */ double drdM = sqrt(sum2/np)/eps; /* Avoid values that are too large or too small: */ drdM = hypot(drdM, 1.0e-6*mag); drdM = 1/(hypot(1/drdM, 1.0e-6*mag)); /* Store it: */ G->c[i][j] = drdM; } } } } void adjust_params(int n, LR2 *p1, LR2 *p2, stitch_params_t *L_p, double p1[], double p2[]) { // Given n pairs of corresponding points (p1[i], p2[i]), each on one // image (in pixels), finds the parameters of a perspective // correspondence that maps points of image 1 to points of image 2. // In the process, the procedure also determines the Z coordinates // (in pixels) p1[i] and p2[i] of each point, relative to the // corresponding image plane, before the perspective transformation. // Assumes that *L_p, *p1[i], and *p2[i] already contain initial // guesses for the unknown parameters. // Mismatch and gradient at current state: double S; stitch_params_t DS_DL; double *DS_Dzt1 = (double*)check_null(malloc(n*sizeof(double))); double *DS_Dzt2 = (double*)check_null(malloc(n*sizeof(double))); // Minimization direction: stitch_params_t UL; double *Uzt1 = (double*)check_null(malloc(n*sizeof(double))); double *Uzt2 = (double*)check_null(malloc(n*sizeof(double))); // Tentative second probe state: stitch_params_t L_b; double *p1_b = (double*)check_null(malloc(n*sizeof(double))); double *p2_b = (double*)check_null(malloc(n*sizeof(double))); // Mismatch and gradient at tentative second state: double S_b; // stitch_params_t DS_DL_b; // double *DS_Dzt1_b = (double*)check_null(malloc(n*sizeof(double))); // double *DS_Dzt2_b = (double*)check_null(malloc(n*sizeof(double))); // double *t; // Parameter change in last descending step: double ch_abs = 10000.0; double ch_rel = 10000.0; // Maximum parameter change in each component: double max_ch_abs = 300; double max_ch_rel = log(2); auto void pack_params(int n, double x[], ); auto double goal_f(int n, double x[]); auto double goal_f(int n, double x[]); double goal_f(int n, double x[]) { stm_compute_mismatch(TRUE, TRUE, n, p1, p2, L_p, p1, p2, &S, &DS_DL, DS_Dzt1, DS_Dzt2); } while ((ch_rel > 0.001) || (ch_abs > 0.01)) { // stm_test_gradient(n, p1, p2, L_p, p1, p2); stm_compute_mismatch(TRUE, TRUE, n, p1, p2, L_p, p1, p2, &S, &DS_DL, DS_Dzt1, DS_Dzt2); select_direction(n, p1, p2, L_p, p1, p2, &DS_DL, DS_Dzt1, DS_Dzt2, &UL, Uzt1, Uzt2); fprintf(stderr, "direction = \n"); stm_print_corresp(stderr, n, &UL, Uzt1, Uzt2); { // Try to find the valley bottom in the gradient direction: double b = stm_gradient_dot(n, &DS_DL, DS_Dzt1, DS_Dzt2, &UL, Uzt1, Uzt2); double h = 0.1 * 2.0 * S/b; int tries = 0; int max_tries = 20; int k; S_b = 2.0 * S + 1.0; while (S_b >= S) { fprintf(stderr, "modifying: h = %12.8f", h); L_b = (*L_p); for (k = 0; k < n; k++) { p1_b[k] = p1[k]; p2_b[k] = p2[k]; } stm_modify_params(h, &ch_rel, &ch_abs, max_ch_rel, max_ch_abs, n, &(L_b), p1_b, p2_b, &UL, Uzt1, Uzt2); fprintf(stderr, " ch_rel = %12.8f ch_abs = %12.8f\n", ch_rel, ch_abs); stm_compute_mismatch(FALSE, FALSE, n, p1, p2, &(L_b), p1_b, p2_b, &S_b, NULL, NULL, NULL); tries++; if (S_b >= S) { double a = (S_b - S + h*b)/(h*h); double h_min_est = b/a/2.0; if (h_min_est >= 0.9375*h) { h = 0.9375*h; fprintf(stderr, ">"); } else if (h_min_est <= 0.0625*h) { h = 0.0625*h; fprintf(stderr, "<"); } else { h = h_min_est; fprintf(stderr, "|"); } } else if (tries > max_tries) { fprintf(stderr, "*** unimin: %d attempts - giving up ***\n", tries); exit(1); } } fprintf(stderr, "×"); // Move to improved state: (*L_p) = L_b; for (k = 0; k < n; k++) { p1[k] = p1_b[k]; p2[k] = p2_b[k]; } // S_a = S_b; // DS_DL_a = DS_DL_b; // t = DS_Dzt1_a; DS_Dzt1_a = DS_Dzt1_b; DS_Dzt1_b = t; // t = DS_Dzt2_a; DS_Dzt2_a = DS_Dzt2_b; DS_Dzt2_b = t; } } } void select_direction( int n, LR2 *p1, LR2 *p2, stitch_params_t *L_p, double p1[], double p2[], stitch_params_t *DS_DL_p, double DS_Dzt1[], double DS_Dzt2[], stitch_params_t *UL_p, double Uzt1[], double Uzt2[]) { double D1, D2 = 0; double x1, y1, z1, x2, y2, z2; int i,j,k; // Compute the approximate mean spread of the points on each side: for (k = 0; k < n; k++) { x1 = p1[k].c[0]; y1 = p1[k].c[1]; z1 = p1[k]; D1 += x1*x1 + y1*y1 + z1*z1; x2 = p2[k].c[0]; y2 = p2[k].c[1]; z2 = p2[k]; D2 += x2*x2 + y2*y2 + z2*z2; } D1 = sqrt(D1/n); D2 = sqrt(D2/n); // Use the gradient direction, but deflate `R' and `m' by `D1,D2': { double m = DS_DL_p->m; double fR = (D1*m + D2/m); double fm = (D1 + D2/(m*m)); (*UL_p) = (*DS_DL_p); for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { UL_p->R.c[3*i+j] /= fR; } } UL_p->m /= fm; for (k = 0; k < n; k++) { Uzt1[k] = DS_Dzt1[k]; Uzt2[k] = DS_Dzt2[k]; } } } double image_stitch_translation_scale(r2_vec_t *p1, r2_vec_t *p2) { /* A change of {eps} in the linear submatrix elements (assumed to be near the identity) implies a change of about {|p|} in the image of a point {p}. */ int np = p1->ne; assert(np == p2->ne); r2_t bar1, bar2; image_stitch_bar(p1, &bar1); image_stitch_bar(p2, &bar2); int k; double maxrsq = 0.0; for (k = 0; k < np; k++) { r2_t q1, q2; r2_sub(&(p1->e[k]), &bar1, &q1); r2_sub(&(p2->e[k]), &bar2, &q2); double r1sq = r2_norm_sqr(&q1); double r2sq = r2_norm_sqr(&q2); maxrsq = fmax(maxrsq, fmax(r1sq, r2sq)); } return sqrt(maxrsq); } double image_stitch_perspective_scale(r2x2_t *M, r2_vec_t *p1, r2_vec_t *p2) { /* We assume that the optimum matrix has a linear submatrix that is close to the identity and perspective elements that are close to zero. Then change of {eps} in the linear submatrix elements implies a change of about {|p|} in the image of a point {p}; whereas a change of {del} in the perspective elements implies a change of {del*|p|^2} in the image. Hence we set the perspective scale factor to the reciprocal of the root-mean-square of the point norms. */ int k; double sumsq = 0.0; for (k = 0; k < np; k++) { r2_t *q1 = &(p1->e[k]); r2_t *q2 = &(p2->e[k]); double r1sq = r2_norm_sqr(q1); double r2sq = r2_norm_sqr(q2); sumsq += r1sq + r2sq; } return 1.0/sqrt(sumsq/(2*np)); }