001: /*
002: * $RCSfile: PolyWarpSolver.java,v $
003: *
004: * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved.
005: *
006: * Use is subject to license terms.
007: *
008: * $Revision: 1.2 $
009: * $Date: 2007/08/29 23:08:09 $
010: * $State: Exp $
011: */
012: package com.sun.media.jai.util;
013:
014: import java.util.Random;
015:
016: /**
017: * A utility class to fit a polynomial to a set of corresponding
018: * points in the source and destination images of a warp. The core is
019: * based on a public-domain Fortran utility routine for singular-value
020: * decomposition.
021: *
022: * @since EA2
023: *
024: */
025: public class PolyWarpSolver {
026:
027: private static double sign(double a, double b) {
028: a = Math.abs(a);
029: if (b >= 0.0F) {
030: return a;
031: } else {
032: return -a;
033: }
034: }
035:
036: private static final double square(double x) {
037: return x * x;
038: }
039:
040: private static final double sqrt(double x) {
041: return (double) Math.sqrt(x);
042: }
043:
044: private static final double hypot(double x, double y) {
045: double xabs = Math.abs(x);
046: double yabs = Math.abs(y);
047:
048: if (xabs > yabs) {
049: return xabs * sqrt(square(yabs / xabs) + 1.0F);
050: } else if (yabs != 0.0F) {
051: return yabs * sqrt(square(xabs / yabs) + 1.0F);
052: } else {
053: return xabs;
054: }
055: }
056:
057: /* Multiply A * B^T */
058: public static double[][] matmul_t(double[][] A, double[][] B) {
059: int rowsA = A.length;
060: int colsA = A[0].length;
061:
062: int rowsB = B[0].length;
063: int colsB = B.length;
064:
065: // Must have colsA == rowsB
066:
067: double[][] out = new double[rowsA][colsB];
068:
069: for (int i = 0; i < rowsA; i++) {
070: double[] outi = out[i];
071: double[] Ai = A[i];
072:
073: for (int j = 0; j < colsB; j++) {
074: double tmp = 0.0F;
075: for (int k = 0; k < colsA; k++) {
076: tmp += Ai[k] * B[j][k];
077: }
078: outi[j] = tmp;
079: }
080: }
081:
082: return out;
083: }
084:
085: /**
086: * Performs Singular-Value Decomposition on a given matrix. The
087: * number of rows of the matrix must be greater than or equal to
088: * the number of columns.
089: *
090: * <p> When the routine completes, the product U*diag(W)*V^T
091: * will be equal to the input matrix A. U will be column-orthogonal
092: * and V will be fully orthogonal. The elements of W will be positive
093: * or zero.
094: *
095: * <p> From the comments in the original Fortran version contained
096: * in the Eispack library:
097: *
098: * <pre>
099: * c this subroutine is a translation of the algol procedure svd,
100: * c num. math. 14, 403-420(1970) by golub and reinsch.
101: * c handbook for auto. comp., vol ii-linear algebra, 134-151(1971).
102: * c Questions and comments should be directed to Alan K. Cline,
103: * c Pleasant Valley Software, 8603 Altus Cove, Austin, TX 78759.
104: * c Electronic mail to cline@cs.utexas.edu.
105: * c
106: * c this version dated january 1989. (for the IBM 3090vf)
107: * </pre>
108: *
109: * @param a the input matrix to be decomposed of size m x n.
110: * @param w an empty vector of length n to be filled in.
111: * @param u an empty matrix of size m x n to be filled in.
112: * @param v an empty matrix of size n x n to be filled in.
113: * @return true if convergence is acheived within 30 iterations.
114: */
115: private static boolean SVD(double[][] a, double[] w, double[][] u,
116: double[][] v) {
117: int i, j, k, l, m, n, i1, k1, l1, mn, its;
118: double c, f, g, h, s, x, y, z, tst1, tst2, scale;
119: double fabs, gabs, habs;
120:
121: l = 0;
122: l1 = 0;
123: m = a.length;
124: n = a[0].length;
125:
126: double[] rv1 = new double[n];
127:
128: for (i = 0; i < m; i++) {
129: for (j = 0; j < n; j++) {
130: u[i][j] = a[i][j];
131: }
132: }
133:
134: g = 0.0F;
135: scale = 0.0F;
136: x = 0.0F;
137:
138: for (i = 0; i < n; i++) {
139: l = i + 1;
140: rv1[i] = scale * g;
141: g = 0.0F;
142: s = 0.0F;
143: scale = 0.0F;
144:
145: if (i < m) {
146: for (k = i; k < m; k++) {
147: scale += Math.abs(u[k][i]);
148: }
149:
150: if (scale != 0.0F) {
151: for (k = i; k < m; k++) {
152: u[k][i] /= scale;
153: s += square(u[k][i]);
154: }
155:
156: f = u[i][i];
157: g = -sign(sqrt(s), f);
158: h = f * g - s;
159: u[i][i] = f - g;
160:
161: for (j = l; j < n; j++) {
162: s = 0.0F;
163:
164: for (k = i; k < m; k++) {
165: s += u[k][i] * u[k][j];
166: }
167: f = s / h;
168: for (k = i; k < m; k++) {
169: u[k][j] += f * u[k][i];
170: }
171: }
172:
173: for (k = i; k < m; k++) {
174: u[k][i] *= scale;
175: }
176: }
177: }
178:
179: w[i] = scale * g;
180: g = 0.0F;
181: s = 0.0F;
182: scale = 0.0F;
183:
184: if ((i < m) && (i != n - 1)) {
185: for (k = l; k < n; k++) {
186: scale += Math.abs(u[i][k]);
187: }
188:
189: if (scale != 0.0F) {
190: for (k = l; k < n; k++) {
191: u[i][k] /= scale;
192: s += square(u[i][k]);
193: }
194:
195: f = u[i][l];
196: g = -sign(sqrt(s), f);
197: h = f * g - s;
198: u[i][l] = f - g;
199:
200: for (k = l; k < n; k++) {
201: rv1[k] = u[i][k] / h;
202: }
203:
204: for (j = l; j < m; j++) {
205: s = 0.0F;
206:
207: for (k = l; k < n; k++) {
208: s += u[j][k] * u[i][k];
209: }
210:
211: for (k = l; k < n; k++) {
212: u[j][k] += s * rv1[k];
213: }
214: }
215:
216: for (k = l; k < n; k++) {
217: u[i][k] *= scale;
218: }
219:
220: }
221: }
222:
223: x = Math.max(x, Math.abs(w[i]) + Math.abs(rv1[i]));
224: }
225:
226: for (i = n - 1; i >= 0; i--) {
227: if (i != n - 1) {
228: if (g != 0.0F) {
229: for (j = l; j < n; j++) {
230: v[j][i] = (u[i][j] / u[i][l]) / g;
231: }
232:
233: for (j = l; j < n; j++) {
234: s = 0.0F;
235: for (k = l; k < n; k++) {
236: s += u[i][k] * v[k][j];
237: }
238: for (k = l; k < n; k++) {
239: v[k][j] += s * v[k][i];
240: }
241: }
242: }
243:
244: for (j = l; j < n; j++) {
245: v[i][j] = v[j][i] = 0.0F;
246: }
247: }
248:
249: v[i][i] = 1.0F;
250: g = rv1[i];
251: l = i;
252: }
253:
254: mn = Math.min(m, n);
255:
256: for (i = mn - 1; i >= 0; i--) {
257: l = i + 1;
258: g = w[i];
259:
260: if (i != n - 1) {
261: for (j = l; j < n; j++) {
262: u[i][j] = 0.0F;
263: }
264: }
265:
266: if (g != 0.0F) {
267: if (i != mn - 1) {
268: for (j = l; j < n; j++) {
269: s = 0.0F;
270:
271: for (k = l; k < m; k++) {
272: s += u[k][i] * u[k][j];
273: } // 440
274: f = (s / u[i][i]) / g;
275: for (k = i; k < m; k++) {
276: u[k][j] += f * u[k][i];
277: }
278: }
279: }
280:
281: for (j = i; j < m; j++) {
282: u[j][i] /= g;
283: }
284: } else {
285: for (j = i; j < m; j++) {
286: u[j][i] = 0.0F;
287: }
288: }
289: u[i][i] += 1.0F;
290: }
291:
292: tst1 = x;
293:
294: for (k = n - 1; k >= 0; k--) {
295: k1 = k - 1;
296: its = 0;
297:
298: while (true) {
299: boolean flag = true;
300:
301: for (l = k; l >= 0; l--) {
302: l1 = l - 1;
303: tst2 = tst1 + Math.abs(rv1[l]);
304: if (tst2 == tst1) {
305: flag = false;
306: break;
307: }
308:
309: tst2 = tst1 + Math.abs(w[l1]);
310: if (tst2 == tst1) {
311: flag = true;
312: break;
313: }
314: }
315:
316: if (flag) {
317: c = 0.0F;
318: s = 1.0F;
319:
320: for (i = l; i < k + 1; i++) {
321: f = s * rv1[i];
322: rv1[i] *= c;
323:
324: tst2 = tst1 + Math.abs(f);
325: if (tst2 != tst1) {
326: g = w[i];
327:
328: h = hypot(f, g);
329: w[i] = h;
330: c = g / h;
331: s = -f / h;
332:
333: for (j = 0; j < m; j++) {
334: y = u[j][l1];
335: z = u[j][i];
336: u[j][l1] = y * c + z * s;
337: u[j][i] = -y * s + z * c;
338: }
339: }
340: }
341: }
342:
343: z = w[k];
344:
345: if (l == k) {
346: if (z < 0.0F) {
347: w[k] = -z;
348: for (j = 0; j < n; j++) {
349: v[j][k] = -v[j][k];
350: }
351: }
352: break;
353: }
354:
355: if (its == 30) {
356: return false;
357: }
358:
359: ++its;
360:
361: x = w[l];
362: y = w[k1];
363: g = rv1[k1];
364: h = rv1[k];
365: f = 0.5F * (((g + z) / h) * ((g - z) / y) + y / h - h
366: / y);
367:
368: g = hypot(f, 1.0F);
369: f = x - (z / x) * z + (h / x)
370: * (y / (f + sign(g, f)) - h);
371:
372: c = 1.0F;
373: s = 1.0F;
374:
375: for (i1 = l; i1 <= k1; i1++) {
376: i = i1 + 1;
377: g = rv1[i];
378: y = w[i];
379: h = s * g;
380: g = c * g;
381:
382: z = hypot(f, h);
383: rv1[i1] = z;
384: c = f / z;
385: s = h / z;
386: f = x * c + g * s;
387: g = -x * s + g * c;
388: h = y * s;
389: y = y * c;
390:
391: for (j = 0; j < n; j++) {
392: x = v[j][i1];
393: z = v[j][i];
394: v[j][i1] = x * c + z * s;
395: v[j][i] = -x * s + z * c;
396: }
397:
398: z = hypot(f, h);
399: w[i1] = z;
400:
401: if (z != 0.0F) {
402: c = f / z;
403: s = h / z;
404: }
405:
406: f = c * g + s * y;
407: x = -s * g + c * y;
408:
409: for (j = 0; j < m; j++) {
410: y = u[j][i1];
411: z = u[j][i];
412: u[j][i1] = y * c + z * s;
413: u[j][i] = -y * s + z * c;
414: }
415: }
416:
417: rv1[l] = 0.0F;
418: rv1[k] = f;
419: w[k] = x;
420: }
421: }
422:
423: return true;
424: }
425:
426: /**
427: *
428: * @param sourceCoords a double array containing the source coordinates
429: * x_0, y_0, x_1, y_1, ...
430: * @param destCoords a double array containing the source coordinates
431: * x_0, y_0, x_1, y_1, ...
432: * @return the best-fit coefficients for a bivariate polynomial of the
433: * given degree mapping the destination points into the source
434: * points. The coefficients for the X polynomial are returned
435: * first, followed by those for the Y polynomial.
436: */
437: public static float[] getCoeffs(float[] sourceCoords,
438: int sourceOffset, float[] destCoords, int destOffset,
439: int numCoords, float preScaleX, float preScaleY,
440: float postScaleX, float postScaleY, int degree) {
441: int i, j, k;
442:
443: int equations = numCoords / 2;
444:
445: /*
446: for (i = 0; i < numCoords; i++) {
447: System.out.println("sourceCoords[" + i + "] = " +
448: sourceCoords[i + sourceOffset]);
449: System.out.println("destCoords[" + i + "] = " +
450: destCoords[i + destOffset]);
451: }
452: */
453:
454: // Number of unknowns
455: int unknowns = (degree + 1) * (degree + 2) / 2;
456: float[] out = new float[2 * unknowns];
457:
458: // Special case for 3-point affine mapping
459: if ((degree == 1) && (numCoords == 3)) {
460: double x0, x1, x2, y0, y1, y2;
461: double u0, u1, u2, v0, v1, v2;
462:
463: x0 = sourceCoords[0] / postScaleX;
464: y0 = sourceCoords[1] / postScaleY;
465: x1 = sourceCoords[2] / postScaleX;
466: y1 = sourceCoords[3] / postScaleY;
467: x2 = sourceCoords[4] / postScaleX;
468: y2 = sourceCoords[5] / postScaleY;
469:
470: u0 = destCoords[0] * preScaleX;
471: v0 = destCoords[1] * preScaleY;
472: u1 = destCoords[2] * preScaleX;
473: v1 = destCoords[3] * preScaleY;
474: u2 = destCoords[4] * preScaleX;
475: v2 = destCoords[5] * preScaleY;
476:
477: double v0mv1 = v0 - v1;
478: double v1mv2 = v1 - v2;
479: double v2mv0 = v2 - v0;
480: double u1mu0 = u1 - u0;
481: double u2mu1 = u2 - u1;
482: double u0mu2 = u0 - u2;
483: double u1v2mu2v1 = u1 * v2 - u2 * v1;
484: double u2v0mu0v2 = u2 * v0 - u0 * v2;
485: double u0v1mu1v0 = u0 * v1 - u1 * v0;
486: double invdet = 1.0F / (u0 * (v1mv2) + v0 * (u2mu1) + (u1v2mu2v1));
487:
488: out[0] = (float) (((v1mv2) * x0 + (v2mv0) * x1 + (v0mv1)
489: * x2) * invdet);
490: out[1] = (float) (((u2mu1) * x0 + (u0mu2) * x1 + (u1mu0)
491: * x2) * invdet);
492: out[2] = (float) (((u1v2mu2v1) * x0 + (u2v0mu0v2) * x1 + (u0v1mu1v0)
493: * x2) * invdet);
494: out[3] = (float) (((v1mv2) * y0 + (v2mv0) * y1 + (v0mv1)
495: * y2) * invdet);
496: out[4] = (float) (((u2mu1) * y0 + (u0mu2) * y1 + (u1mu0)
497: * y2) * invdet);
498: out[5] = (float) (((u1v2mu2v1) * y0 + (u2v0mu0v2) * y1 + (u0v1mu1v0)
499: * y2) * invdet);
500:
501: return out;
502: }
503:
504: double[][] A = new double[equations][unknowns];
505:
506: /*
507: Fill in A with:
508:
509: 1 x_0 y_0 ... x_0*y_0^(n-1) y_0^n
510: 1 x_1 y_1 ... x_1*y_1^(n-1) y_1^n
511: ...
512: 1 x_(k-1) y_(k-1) ... x_(k-1)*y_(k-1)^(n-1) y_(k-1)^n
513:
514: The height of the matrix is equal to the number of equations
515: The width of the matrix is equal to the number of unknowns
516: */
517:
518: double[] xpow = new double[degree + 1];
519: double[] ypow = new double[degree + 1];
520:
521: for (i = 0; i < equations; i++) {
522: double[] Ai = A[i];
523: double x = destCoords[2 * i + destOffset] * preScaleX;
524: double y = destCoords[2 * i + 1 + destOffset] * preScaleY;
525:
526: double xtmp = 1.0F;
527: double ytmp = 1.0F;
528: for (int d = 0; d <= degree; d++) {
529: xpow[d] = xtmp;
530: ypow[d] = ytmp;
531: xtmp *= x;
532: ytmp *= y;
533: }
534:
535: int index = 0;
536: for (int deg = 0; deg <= degree; deg++) {
537: for (int ydeg = 0; ydeg <= deg; ydeg++) {
538: Ai[index++] = xpow[deg - ydeg] * ypow[ydeg];
539: }
540: }
541: }
542:
543: double[][] V = new double[unknowns][unknowns];
544: double[] W = new double[unknowns];
545: double[][] U = new double[equations][unknowns];
546: SVD(A, W, U, V);
547:
548: // Multiply the columns of V by the inverted diagonal entries of W
549: for (j = 0; j < unknowns; j++) {
550: double winv = W[j];
551: if (winv != 0.0) {
552: winv = 1.0F / winv;
553: }
554: for (i = 0; i < unknowns; i++) {
555: V[i][j] *= winv;
556: }
557: }
558:
559: // Multiply V by U^T
560: double[][] VWINVUT = matmul_t(V, U); // unknowns x equations
561:
562: // Multiply VWINVUT by source coords to yield output coefficients
563: for (i = 0; i < unknowns; i++) {
564: double tmp0 = 0;
565: double tmp1 = 0;
566: for (j = 0; j < equations; j++) {
567: double val = VWINVUT[i][j];
568: tmp0 += val * sourceCoords[2 * j + sourceOffset]
569: / postScaleX;
570: tmp1 += val * sourceCoords[2 * j + 1 + sourceOffset]
571: / postScaleY;
572: }
573: out[i] = (float) tmp0;
574: out[i + unknowns] = (float) tmp1;
575: }
576:
577: return out;
578: }
579:
580: /* Code for unit test */
581:
582: private static Random myRandom = new Random(0);
583: private static double c0[] = new double[6];
584: private static double c1[] = new double[6];
585: private static double preScaleX;
586: private static double preScaleY;
587: private static double postScaleX;
588: private static double postScaleY;
589: private static double noise = 0.0F;
590:
591: private static float xpoly(float x, float y) {
592: x *= (float) preScaleX;
593: y *= (float) preScaleY;
594: return (float) (postScaleX * (c0[0] + c0[1] * x + c0[2] * y
595: + c0[3] * x * x + c0[4] * x * y + c0[5] * y * y + myRandom
596: .nextDouble()
597: * noise));
598: }
599:
600: private static float ypoly(float x, float y) {
601: x *= (float) preScaleX;
602: y *= (float) preScaleY;
603: return (float) (postScaleY * (c1[0] + c1[1] * x + c1[2] * y
604: + c1[3] * x * x + c1[4] * x * y + c1[5] * y * y + myRandom
605: .nextDouble()
606: * noise));
607: }
608:
609: private static void doTest(int equations, boolean print) {
610: // Make up a pair of random polynomials
611: for (int i = 0; i < 6; i++) {
612: c0[i] = myRandom.nextDouble() * 100.0F;
613: c1[i] = myRandom.nextDouble() * 100.0F;
614: }
615:
616: // Make up random pre- and postScales:
617: preScaleX = myRandom.nextDouble() + 2.0; // Value between 2 and 3
618: preScaleY = myRandom.nextDouble() + 3.0; // Value between 3 and 4
619: postScaleX = myRandom.nextDouble() + 4.0; // Value between 4 and 5
620: postScaleY = myRandom.nextDouble() + 5.0; // Value between 5 and 6
621:
622: // Make up random destination coordinates
623: float[] destCoords = new float[2 * equations];
624: for (int i = 0; i < 2 * equations; i++) {
625: destCoords[i] = myRandom.nextFloat() * 100.0F;
626: }
627:
628: // Compute corresponding source coordinates
629: float[] sourceCoords = new float[2 * equations];
630: for (int i = 0; i < equations; i++) {
631: sourceCoords[2 * i] = xpoly(destCoords[2 * i],
632: destCoords[2 * i + 1]);
633: sourceCoords[2 * i + 1] = ypoly(destCoords[2 * i],
634: destCoords[2 * i + 1]);
635: }
636:
637: // Recover the polynomials using the SVD algorithm
638: float[] coeffs = getCoeffs(sourceCoords, 0, destCoords, 0,
639: sourceCoords.length, (float) preScaleX,
640: (float) preScaleY, (float) postScaleX,
641: (float) postScaleY, 2);
642:
643: // Print the results
644: if (print) {
645: System.out.println("Using " + equations + " equations:");
646: for (int i = 0; i < 6; i++) {
647: System.out.println("c0[" + i + "] = " + c0[i]
648: + ", recovered as " + coeffs[i] + " (ratio = "
649: + (c0[i] / coeffs[i]) + ")");
650: System.out.println("c1[" + i + "] = " + c1[i]
651: + ", recovered as " + coeffs[i + 6]
652: + " (ratio = " + (c1[i] / coeffs[i + 6]) + ")");
653: }
654: }
655: }
656:
657: public static void main(String[] args) {
658: for (int times = 0; times < 3; times++) {
659: doTest(6 + 50 * times, true);
660: System.out.println();
661: }
662:
663: int trials = 10000;
664: int points = 6;
665:
666: long startTime = System.currentTimeMillis();
667: for (int times = 0; times < trials; times++) {
668: doTest(points, false);
669: }
670: long endTime = System.currentTimeMillis();
671: System.out.println("Did " + trials + " " + points
672: + "-point solutions in "
673: + ((endTime - startTime) / 1000.0F) + " seconds.");
674: System.out.println("Rate = "
675: + (trials * 1000.F / (endTime - startTime))
676: + " trials/second");
677: }
678: }
|