/* Last edited on 2024-09-07 18:31:49 by stolfi */ /* ---------------------------------------------------------------------- */ /* from hr2_pmap_opt.{h,c} */ void hr2_pmap_opt_degrees_from_type ( r3x3_t *M, r3x3_t *R, double *Mp[], double Re[], int32_t *nvP ) { int32_t nv = 0; for (int32_t i = 0; i < 3; i++) { for (int32_t j = 0; j < 3; j++) { double *Rp = &(R->c[i][j]); /* Pointer to element of {*R}. */ assert((*Rp) >= 0.0); if ((*Rp) != 0) { demand(nv <= 8, "at least one element of {M} must be fixed}"); Mp[nv] = &(M->c[i][j]); /* Pointer to element of {*M}. */ Re[nv] = (*Rp); nv++; } } } (*nvP) = nv; } double *Ap[8]; /* Pointers to adjustable elements of {A.dir}. */ double Re[8]; /* Max adjustment amount of each element. */ hr2_pmap_opt_degrees_from_type(&(A->dir), R, Ap, Re, &nv); /* ---------------------------------------------------------------------- */ /* From {r2_align.{h,c} */ double r2_align_norm_sqr (int32_t ni, r2_t p[]); /* Given an alignment vector {p} with {ni} components, returns the square of its total Euclidean norm. Namely, returns the sum of squares of all {p[i].c[j]} for {i} in {0..ni-1]} and {j} in {0..1}. */ double r2_align_norm_sqr(int32_t ni, r2_t p[]) { double sum2 = 0.0; /* Sum of squares of all coordinates. */ for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double pij = p[i].c[j]; sum2 += pij*pij; } } return sum2; } void r2_align_throw_ortho_disp_vector(int32_t ni, r2_t arad[], int32_t k, r2_t U[]); /* Assumes that the rows of {U} are {k} delta vectors with {ni} points each, orthonormal, balanced, and conformal to {arad}, and {U} has space for at least one more row. Stores into row {k} of {U} another alignment vector that is normalized, balanced, conformal to {arad}, and orthogonal to all of them. Specifically, assumes that the delta vector basis element {u[kb]} with index {kb} is points {u[kb][i] = U[kb*ni + i]} for {i} in {0..ni-1}. The parameter {ni} must be at least 2, and {k} must be less than {nd} where {nv} is the number of non-zero coordinates in {arad}. */ void r2_align_throw_ortho_disp_vector(int32_t ni, r2_t arad[], int32_t k, r2_t H[]) { bool_t debug = TRUE; if (debug) { fprintf(stderr, " computing H[%d]\n", k); } r2_t *hk = &(H[ni*k]); int32_t nit = 0; /* Counts iterations, for safety. */ while (TRUE) { nit++; assert(nit < 1000); /* Should never happen if {k < nd} */ /* Generate a vector {hk} uniformly distributed in the unit ball that is not too short: */ double rmin = 0.5; /* To reduce the effect of roundoff noise on normalization. */ r2_align_throw_ball_vector(ni, rmin, 1.0, hk); /* Project {hk} perpendicular to coordinates where {arad} is zero: */ for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double rij = arad[i].c[j]; demand (rij >= 0, "invalid {arad}"); if (rij == 0.0) { hk[i].c[j] = 0.0; } } } /* Project {hk} perpendicular to the all-ones vectors, preserving conformity: */ for (int32_t j = 0; j < 2; j++) { double sum = 0.0; int32_t nv = 0; for (int32_t i = 0; i < ni; i++) { double rij = arad[i].c[j]; if (rij != 0.0) { sum += hk[i].c[j]; nv++; } } if (nv > 0) { double avg = sum/nv; for (int32_t i = 0; i < ni; i++) { double rij = arad[i].c[j]; if (rij != 0.0) { hk[i].c[j] -= avg; } } } } /* Project {hk} perpendicular to the previous delta vectors. */ /* Since the previous vectors are conformal to {arad} and balanced, the projection preserves these properties: */ for (int32_t r = 0; r < k; r++) { r2_t *hr = &(H[ni*r]); double sdot = r2_align_dot(ni, hk, hr); for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { hk[i].c[j] -= sdot*hr[i].c[j]; } } } /* Check if norm is still large enough: */ double sum2 = r2_align_norm_sqr(ni, hk); if (sum2 >= rmin*rmin) { /* Normalize and return this vector: */ double norm = sqrt(sum2); for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { hk[i].c[j] /= norm; } } return; } /* Remaining vector {hk} was too short. Try again. */ } } void r2_align_points_to_vars(int32_t ni, r2_t p[], r2_t arad[], r2_t ctr[], int32_t nv, double y[]) { int32_t k = 0; for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double rij = arad[i].c[j]; if (rij != 0) { double c0 = (ctr != NULL ? ctr[i].c[j] : 0.0); demand(k < nv, "{nv} too small"); y[k] = p[i].c[j] - c0; k++; } } } demand(k == nv, "wrong {nv}"); } void r2_align_vars_to_points(int32_t nv, double y[], int32_t ni, r2_t arad[], r2_t ctr[], r2_t p[]) { int32_t k = 0; for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double rij = arad[i].c[j]; if (rij != 0) { double c0 = (ctr != NULL ? ctr[i].c[j] : 0.0); demand(k < nv, "{nv} too small"); p[i].c[j] = c0 + y[k]; k++; } } } demand(k == nv, "wrong {nv}"); } if (ctvars) { int32_t nc = 2*ni; i2_t nv = r2_align_count_variable_coords(ni, p); int32_t nd = r2_align_count_degrees_of_freedom (nv, bal); fprintf(wr, " nc = %d nz = %d nv = %d+%d = %d nd = %d\n", nc, nz, nv.c[0], nv.c[1], nv.c[0]+nv.c[1], nd); } /* BASIC DOMAIN ELLIPSOID Searches for an optimal alignment vector are confined to a /basic ellipsoid/ {\RE \sub \RC}, defined by two alignment vectors {ctr[0..ni-1]} (the /domain center/) and {arad[0..ni-1]} (the /radius vector/). The coordinates {arad[i].c[j]} of the radius vector must be non-negative. In the rest of these comments, the vectors {ctr} and {arad} are assumed fixed. The ellipsoid {\RE} has the main axes aligned with the coordinate axes of {\RC}, and its radius along the axis {i,j} is {arad[i].c[j]}. Namely, it consists of all alignment vectors {p} such that the sum of the squares of {(p[i].c[j] - ctr[i].c[j])/arad[i].c[j]}, over all {i,j} such that {arad[i].c[j]!=0}, is at most 1. */ void r2_align_points_to_vars (int32_t ni, r2_t p[], r2_t arad[], r2_t ctr[], int32_t nv, double y[]); /* Stores the non-fixed coordinates of the delta vector from {p[0..ni-1]} to {ctr[0..ni-1]}, into the vector {y[0..nv-1]}. The number {nv} must be the total number of non-fixed coordinates , that is, the number of non-zero coordinates {arad[i].c[j]}. If {ctr} is {NULL}, assumes it is all zeros. */ void r2_align_vars_to_points (int32_t nv, double y[], int32_t ni, r2_t arad[], r2_t ctr[], r2_t p[]); /* Stores {ctr[0..ni-1]} plus {y[0..nv-1]} into the non-fixed coordinates of the alignment vector {p[0..ni-1]}. Namely sets {p[i].c[j]} to {ctr[i].c[j] + y[k]} whenever {arad[i].c[j]} is nonzero. The number {nv} must be the total number of non-fixed coordinates , that is, the number of non-zero coordinates {arad[i].c[j]}. If {ctr} is {NULL}, assumes it is all zeros. */ /* First we determine the number {nh} of coordinates of {\RC} that are variable, and assume a map {\SH} from the conformal vectors of {\RC} to {\RR^{nh}} that consists of just copying those coordinates of {p - ctr} to a plain {nh}-vector {h}, in order. The corresponding radii {arad[0..ni-1].c[0..1]} are copied to {hrad[0..nh-1]}. */ if (debug) { fprintf(stderr, " ... extracting the non-zero radii\n"); } int32_t nh = 0; double hrad[2*ni]; /* {h[0..nh-1]} will be the non-zero entries in {arad[0..ni-1]}. */ for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double arij = arad[i].c[j]; if (arij != 0) { hrad[nh] = arij; nh++; } } } assert(nh == nv.c[0] + nv.c[1]); /* ---------------------------------------------------------------------- */ double *hr2_pmap_aff_elem_address(hr2_pmap_t *M, int32_t s); /* Returns the address of element {s} of the affine projective map whose direct matrix is {A}. The elements {A.c[0..2][0]} must be {1,0,0}. The integer {s} must be in {0..5}. For {s} is in {0..3}, returns the addresses of the elements of the linear 2x2 subarray {A.c[1..2][1..2]}, in row-by roe order. For {s} in {4..5}, returns the addresses of the displacement coefficients {A.c[0][1..2]}. */ double *hr2_pmap_aff_elem_address(hr2_pmap_t *M, int32_t s) { r3x3_t *A = &(M->dir); hr2_pmap_aff_norm_check(A); demand((s >= 0) && (s < 6), "invalid elem index"); if (s < 4) { int32_t i = 1 + s / 2, j = 1 + s % 2; return &(A->c[i][j]); } else { int32_t j = 1 + (s - 4); return &(A->c[0][j]); } } /* ---------------------------------------------------------------------- */ /* r2_align.c */ if (debug) { fprintf(stderr, "... finding an orthonormal basis {H} for {\\RU} ...\n"); } r2_t H[nd*ni]; for (int32_t k = 0; k < nd; k++) { r2_t *hk = &(H[k*ni]); r2_align_throw_ortho_disp_vector(ni, arad, k, H); if (debug) { r2_align_print_vector(stderr, ni, "h", k, hk, FALSE); } } if (debug) { fprintf(stderr, "... computing the metric matrix {M} for {\\RF} ...\n"); } double M[nd*nd]; for (int32_t r = 0; r < nd; r++) { r2_t *hr = &(H[r*ni]); for (int32_t s = 0; s <= r; s++) { r2_t *hs = &(H[s*ni]); double sum = 0.0; for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double hrij = hr[i].c[j]; double hsij = hs[i].c[j]; double rij = arad[i].c[j]; if (rij == 0) { assert((hrij == 0) && (hsij == 0)); } else { sum += hrij*hsij/(rij*rij); } } } M[r*nd + s] = sum; M[s*nd + r] = sum; /* Diag is assigned twice, but OK. */ } } if (debug) { fprintf(stderr, "... computing the eigen decomp {S,d} of {M} ...\n"); } double Q[nd*nd]; /* Eigenvector matrix. */ double d[nd]; /* Eigenvalues. */ { /* Convert {M} to tridiag with diagonal {d[0..nd-1]} and subdiagonal {e[1..nd-1]}: */ double e[nd]; /* Sub-diagonal elements of temporary tridiagonal matrix. */ syei_tridiagonalize(nd, M, d, e, Q); /* Compute eigenvalues and eigenvectors from {Q} and tridiag matrix: */ int32_t p; /* Number of eigenvalues computed. */ int32_t absrt = 0; /* Sort eigenvalues by signed value. */ syei_trid_eigen(nd, d, e, Q, &p, absrt); /* Check that all eigenvalues are positive: */ demand(p == nd, "failed to determine eigenvalues of {M}"); } /* Compute the radii {urad[0..nd-1]} of {\RF}: */ for (int32_t k = 0; k < nd; k++) { demand(d[k] > 0.0, "non-positive eigenvalue"); urad[k] = 1.0/sqrt(d[k]); } if (debug) { fprintf(stderr, "... computing the basis {U = Q H} aligned with axes of {\\RF} ...\n"); } for (int32_t r = 0; r < nd; r++) { double *qr = &(Q[r*nd]); r2_t *ur = &(U[r*ni]); for (int32_t i = 0; i < ni; i++) { for (int32_t j = 0; j < 2; j++) { double sum = 0.0; for (int32_t s = 0; s < nd; s++) { r2_t *hs = &(H[s*ni]); double qrs = qr[s]; double hsij = hs[i].c[j]; sum += qrs*hsij; } ur[i].c[j] = sum; } } if (debug) { r2_align_print_vector(stderr, ni, "u", r, ur, FALSE); } if (debug) { fprintf(stderr, "radius {urad[%d]} = %.8f\n", r, urad[r]); } } /* ---------------------------------------------------------------------- */ void pack_parameters(hr2_pmap_t *M, int32_t nx, double x[]) { assert(nx == 8); int32_t ix, iy; int32_t k = 0; for (int32_t ix = 0; ix < 2; ix++) { double px = (ix == 0 ? L1->c[0] : H1->c[0]); for (int32_t iy = 0; iy < 2; iy++) { double py = (iy == 0 ? L1->c[1] : H1->c[1]); r2_t p = (r2_t){{ px, py }}; r2_t q; image_stitch_map_point(&p, &(M->dir), &q); x[k] = q.c[0]; k++; x[k] = q.c[1]; k++; } } } void unpack_parameters(int32_t nx, double x[], hr2_pmap_t *M) { assert(nx == 8); int32_t ix, iy; int32_t k = 0; hr2_point_t ph[4]; for (int32_t ix = 0; ix < 2; ix++) { for (int32_t iy = 0; iy < 2; iy++) { double qx = x[k]; k++; double qy = x[k]; k++; ph[2*iy + ix] = (hr2_point_t){{{ 1, qx, qy }}}; } } hr2_pmap_t R = hr2_pmap_from_four_points(&(ph[0]), &(ph[1]), &(ph[2]), &(ph[3])); (*M) = hr2_pmap_compose(&S, &R); } (*f2AP) = f2(A); /* Current goal function on {A}. */ /* ---------------------------------------------------------------------- */ /* Identify the elements to optimize: */ int32_t nv; double *Ap[8]; /* Pointers to adjustable elements of {A.dir}. */ double Re[8]; /* Max adjustment amount of each element. */ hr2_pmap_from_many_pairs_get_var_elems(&(A->dir), R, Ap, Re, &nv); fprintf(stderr, "%d adjustable elements found\n", nv); assert (nv <= 8); hr2_pmap_t compute_S_map(void) { hr2_point_t ph[4]; int32_t ix, iy; for (int32_t ix = 0; ix < 2; ix++) { double px = (ix == 0 ? L1->c[0] : H1->c[0]); for (int32_t iy = 0; iy < 2; iy++) { double py = (iy == 0 ? L1->c[1] : H1->c[1]); ph[2*iy+ix] = (hr2_point_t){{{ 1, px, py }}}; } } hr2_pmap_t S = hr2_pmap_from_four_points(&(ph[0]), &(ph[1]), &(ph[2]), &(ph[3])); return hr2_pmap_inv(&S); } if (nv > 0) { /* Apply nonlinear optimization. */ /* Save initial elem values (center of search region): */ double Ce[nv]; for (int32_t iv = 0; iv < nv; iv++) { Ce[iv] = (*(Ap[iv])); } /* The optimization variable {x[iv]}, for {iv} in {0..nv-1}, is the difference {((*Ap)[iv]-Ce[iv]} divided by the corresponding radius {Re[iv]}. */ auto void extract_vars(double y[]); /* Converts the mutable elements of {*A} into the optmizaton variables {y[0..nv-1]}. */ auto void store_vars(double y[]); /* Converts the optimization variables {y[0..nv-1]} into the mutable elements of {*A}. */ auto double sve_goal(int32_t nx, double x[]); /* Computes the minimization goal function from the given argument {x[0..nx-1]}. Expects {nx == nv}. Also sets {*A} from {x[0..nx-1]}. */ /* Convert the projective map {*A} to the optimization variables {z[0..nv-1]}: */ double z[nv]; extract_vars(z); /* Compute the initial goal function value: */ double Fz = sve_goal(nv, z); /* Convert the optimization variables {z[0..nv-1]} to projective map {*A}: */ store_vars(z); /* Local implementations: */ void extract_vars(double y[]) { for (int32_t iv = 0; iv < nv; iv++) { assert (Re[iv] != 0); y[iv] = ((*(Ap[iv])) - Ce[iv])/Re[iv]; } } void store_vars(double y[]) { for (int32_t iv = 0; iv < nv; iv++) { (*(Ap[iv])) = Ce[iv] + y[iv]*Re[iv]; } r3x3_inv(&(A->dir), &(A->inv)); } double sve_goal(int32_t nx, double x[]) { assert(nx == nv); /* Convert variables {x[0..nx-1]} to displacements {*A}: */ store_vars(x); /* Evaluate the client function: */ double Q2 = f2(A); (*f2AP) = Q2; return Q2; } } auto void pack_parameters(hr2_pmap_t *M, int32_t nx, double x[]); /* Packs the homogeneous projective map {M}, assumed to be of type {type}, to the parameter vector {x[0..nx-1]}. */ auto void unpack_parameters(int32_t nx, double x[], hr2_pmap_t *M); /* Unpacks the parameter vector {x[0..nx-1]} to the homogeneous map matrix {M}, assumed to be of type {type}. The class of {M} is preserved. */ /* --- internal procs ----------------------------------------------------- */ void pack_parameters(hr2_pmap_t *M, int32_t nx, double x[]) { switch(type) { case hr2_pmap_type_IDENTITY: case hr2_pmap_type_TRANSLATION: /* Should not require optimization: */ demand(FALSE, "unexpected map type"); case hr2_pmap_type_CONGRUENCE: assert(nx == 3); hr2_pmap_opt_congruence_encode(M, x); break; /* case hr2_pmap_type_SIMILARITY: assert(nx == 4); hr2_pmap_opt_similarity_encode(M, x); break; case hr2_pmap_type_AFFINE: assert(nx == 6); hr2_pmap_opt_affine_encode(M, x); break; case hr2_pmap_type_PROJECTIVE: assert(nx == 8); hr2_pmap_opt_general_encode(M, x); break; */ default: demand(FALSE, "invalid map type"); } void unpack_parameters(int32_t nx, double x[], hr2_pmap_t *M) { switch(type) { case hr2_pmap_type_IDENTITY: case hr2_pmap_type_TRANSLATION: /* Should not require optimization: */ demand(FALSE, "unexpected map type"); case hr2_pmap_type_CONGRUENCE: assert(nx == 3); hr2_pmap_opt_congruence_decode(x, M); break; /* case hr2_pmap_type_SIMILARITY: assert(nx == 4); hr2_pmap_opt_similarity_decode(x, M); break; case hr2_pmap_type_AFFINE: assert(nx == 6); hr2_pmap_opt_affine_decode(x, M); break; case hr2_pmap_type_PROJECTIVE: assert(nx == 8); hr2_pmap_opt_general_decode(x, M); break; */ default: demand(FALSE, "invalid map type"); } double goalf(int32_t nx, double x[]) { if (debug) { rn_gen_print(stderr, nx, x, "%8.1f", "\n [ ", " ", " ]\n"); } unpack_parameters(nx, x, M); /* Mean square error on point lists: */ double F = hr2_pmap_mismatch_sqr(&M, np, p1, p2, w); if (debug) { fprintf(stderr, " mean squared error = %13.6e", F); */ /* Deformation penalty: */ /* double dsq1 = image_stitch_deform_sqr(L1, H1, &(M.dir)); double dsq2 = image_stitch_deform_sqr(L2, H2, &(M.inv)); F += 0.0001*(dsq1 + dsq2); if (debug) { fprintf(stderr, " squared deform = %13.6e %13.6e", dsq1, dsq2); } */ if (debug) { fprintf(stderr, " function = %13.6e\n", F); return F; } int32_t nv = hr2_pmap_opt_degrees_of_freedom(type) double x[nx]; /* Initial guess and final optimum parameters. */ pack_parameters(M0, nx, x); double Fx = goalf(nx, x); auto bool_t is_ok(int32_t nx, double x[], double Fx); /* Returns true if the squared error {Fx} is small enough. */ sve_minn_iterate ( nx, &goalf, NULL, x, &Fx, dir, dMax, dBox, rIni, rMin, rMax, stop, maxIters, debug ); bool_t is_ok(int32_t nx, double x[], double Fx) { return Fx < maxErr*maxErr; } /* Unpack projective map: */ unpack_parameters(nx, x, M); /* Compue the final mismatch: */ (*f2AP) = f2(A); double esq = goalf(M); double dMax = 20*sqrt(esq); /* Search radius around initial guess. */ bool_t dBox = FALSE; /* Search in ball, not in box. ??? */ double rIni = 0.250*dMax; /* Initial probe radius. */ double rMin = 0.5; /* Minimum probe radius. */ double rMax = 0.500*dMax; /* Maximum probe radius. */ double stop = 0.1*maxErr; /* Stopping criterion. */ sign_t dir = -1; /* Look for minimum. */ typedef void hr2_pmap_opt_report_proc_t (hr2_pmap_t *M, double F); /* Type of a procedure used by projective map optimization functions to report the probes made during optimization. It is called after every call to the internal goal function. The map {M} is the projective map that was tried, and {F} is the value of the goal function for it. */ /* Initialize constant elements of the incremental map {M}: */ hr2_pmap_t M = hr2_pmap_identity(); /* Scaling factors from the unit cube to the mod range: */ double rad[nv]; for (int32_t iv = 0; iv < nv; iv++) { rad[iv] = maxMod; } double sumA2 = 0; double sumB2 = 0; for (int32_t i = 0; i < 3; i++) for (int32_t j = 0; j < 3; j++) { double Aij = M->dir.c[i][j]; sumA2 += Aij*Aij; double Bij = M->inv.c[i][j]; sumB2 += Bij*Bij; } void hpedt_normalize_matrix(r3x3_t *A); /* Tries to scale all elements of {A} so that element {A[0][0]} becomes 1. If that element is too small, scales so that the sum of squares of elements is 1. if all elements are too small, leaves the matrix unchanged. */ void hpedt_normalize_matrix(r3x3_t *A) { double norm = r3x3_norm(A); double head = fabs(A->c[0][0]); double scale = (head > 0.01*norm ? head : norm); if ((scale == 1.0) || (scale < 1.0e-200)) { return; } for(int32_t i = 0; i < 3; i++) { for (int32_t j = 0; j < 3; j++) { A->c[i][j] /= scale; } } }