001: /*
002: * Geotools2 - OpenSource mapping toolkit
003: * http://geotools.org
004: * (C) 2002-2006, Geotools Project Managment Committee (PMC)
005: *
006: * This library is free software; you can redistribute it and/or
007: * modify it under the terms of the GNU Lesser General Public
008: * License as published by the Free Software Foundation;
009: * version 2.1 of the License.
010: *
011: * This library is distributed in the hope that it will be useful,
012: * but WITHOUT ANY WARRANTY; without even the implied warranty of
013: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
014: * Lesser General Public License for more details.
015: */
016: package org.geotools.referencing.operation.builder;
017:
018: import org.geotools.referencing.datum.BursaWolfParameters;
019: import org.geotools.referencing.operation.matrix.GeneralMatrix;
020: import org.geotools.referencing.operation.transform.GeocentricTranslation;
021: import org.opengis.referencing.FactoryException;
022: import org.opengis.referencing.cs.CartesianCS;
023: import org.opengis.referencing.datum.GeodeticDatum;
024: import org.opengis.referencing.operation.MathTransform;
025: import org.opengis.geometry.DirectPosition;
026: import org.opengis.geometry.MismatchedDimensionException;
027: import org.opengis.geometry.MismatchedReferenceSystemException;
028:
029: // J2SE and extensions
030: import java.util.List;
031: import javax.vecmath.MismatchedSizeException;
032:
033: /**
034: * Builds {@linkplain org.opengis.referencing.operation.MathTransform
035: * MathTransform} setup as BursaWolf transformation from a list of {@linkplain
036: * org.geotools.referencing.operation.builder.MappedPosition MappedPosition}.
037: * The calculation uses least square method. Calculated parameters can be
038: * used for following operations:<p></p>
039: * <p>The equations:<pre> X = q * R * x + T , </pre>Where X
040: * is the Matrix of destination points, q is the scale, R is the rotation
041: * Matrix, x is the Matrix of source points and T is matrix of translation.
042: * Expressing the errors, we get this:<pre> Err = A * Dx + l </pre>where
043: * Err is the Error Matrix, A is Matrix of derivations, Dx is Matrix of
044: * difference changes of 7 parameters, and l is value of DX, DY, DZ for
045: * calculated from approximate values. Using the least square method to
046: * minimalize the errors we get this result:<pre>
047: * Dx = (A<sup>T</sup>A)<sup>-1</sup> A<sup>T</sup>l </pre></p>
048: *
049: * @since 2.4
050: * @source $URL: http://svn.geotools.org/geotools/tags/2.4.1/modules/library/referencing/src/main/java/org/geotools/referencing/operation/builder/BursaWolfTransformBuilder.java $
051: * @version $Id: BursaWolfTransformBuilder.java 24925 2007-03-27 20:12:08Z jgarnett $
052: * @author Jan Jezek
053: *
054: */
055: public class BursaWolfTransformBuilder extends MathTransformBuilder {
056: /** The Geodetic Datum of target reference system */
057: private GeodeticDatum targetDatum;
058:
059: /** Matrix of source points */
060: GeneralMatrix x;
061:
062: /** Matrix of destination points. */
063: GeneralMatrix X;
064:
065: /** Bursa Wolf rotation in arc radians. */
066: private double alfa = 0;
067:
068: /** Bursa Wolf rotation in arc radians. */
069: private double beta = 0;
070:
071: /** Bursa Wolf rotation in arc radians. */
072: private double gamma = 0;
073:
074: /** Bursa Wolf shift in meters. */
075: private double dx = 0;
076:
077: /** Bursa Wolf shift in meters. */
078: private double dy = 0;
079:
080: /** Bursa Wolf shift in meters. */
081: private double dz = 0;
082:
083: /** Bursa Wolf scaling. */
084: private double q = 1;
085:
086: /**
087: * Creates a BursaWolfTransformBuilder.
088: *
089: * @param vectors list of {@linkplain
090: * org.geotools.referencing.operation.builder.MappedPosition MappedPosition}
091: */
092: public BursaWolfTransformBuilder(List /*<MappedPosition>*/vectors)
093: throws MismatchedSizeException,
094: MismatchedDimensionException,
095: MismatchedReferenceSystemException {
096: super .setMappedPositions(vectors);
097:
098: x = new GeneralMatrix(vectors.size(), 3);
099: X = new GeneralMatrix(vectors.size(), 3);
100: x = getx();
101: X = getX();
102: this .getDxMatrix();
103: }
104:
105: /**
106: * Returns the minimum number of points required by this builder,
107: * which is 3.
108: *
109: * @return the minimum number of points required by this builder which is
110: * 3.
111: */
112: public int getMinimumPointCount() {
113: return 3;
114: }
115:
116: /**
117: * Returns the dimension for {@link #getSourceCRS source} and
118: * {@link #getTargetCRS target} CRS, which is 2.
119: *
120: * @return dimension for {@linkplain #getSourceCRS source} and {@link
121: * #getTargetCRS target} CRS, which is 2.
122: */
123: public int getDimension() {
124: return 3;
125: }
126:
127: /**
128: * Returns the required coordinate system type, which is
129: * {@linkplain CartesianCS cartesian CS}.
130: *
131: * @return coordinate system type
132: */
133: public Class /*<? extends CartesianCS>*/getCoordinateSystemType() {
134: return CartesianCS.class;
135: }
136:
137: /**
138: * Fills the x matrix by coordinates of source points.
139: *
140: * @return x matrix.
141: */
142: protected GeneralMatrix getx() {
143: final DirectPosition[] sourcePoints = getSourcePoints();
144: GeneralMatrix x = new GeneralMatrix(3 * sourcePoints.length, 1);
145:
146: for (int j = 0; j < (x.getNumRow()); j = j + 3) {
147: x.setElement(j, 0, sourcePoints[j / 3].getCoordinates()[0]);
148: x.setElement(j + 1, 0,
149: sourcePoints[j / 3].getCoordinates()[1]);
150: x.setElement(j + 2, 0,
151: sourcePoints[j / 3].getCoordinates()[2]);
152: }
153:
154: return x;
155: }
156:
157: /**
158: * Fills the x matrix by coordinates of destination points.
159: *
160: * @return the X matrix
161: */
162: protected GeneralMatrix getX() {
163: final DirectPosition[] sourcePoints = getSourcePoints();
164: final DirectPosition[] targetPoints = getTargetPoints();
165: GeneralMatrix X = new GeneralMatrix(3 * sourcePoints.length, 1);
166:
167: for (int j = 0; j < (X.getNumRow()); j = j + 3) {
168: X.setElement(j, 0, targetPoints[j / 3].getCoordinates()[0]);
169: X.setElement(j + 1, 0,
170: targetPoints[j / 3].getCoordinates()[1]);
171: X.setElement(j + 2, 0,
172: targetPoints[j / 3].getCoordinates()[2]);
173: }
174:
175: return X;
176: }
177:
178: /**
179: * Generates rotation matrix around X axis.
180: *
181: * @return rotation Matrix
182: */
183: protected GeneralMatrix getRalfa() {
184: GeneralMatrix Ralfa = new GeneralMatrix(3, 3);
185: double[] m0 = { 1, 0, 0 };
186: double[] m1 = { 0, Math.cos(alfa), Math.sin(alfa) };
187: double[] m2 = { 0, -Math.sin(alfa), Math.cos(alfa) };
188: Ralfa.setRow(0, m0);
189: Ralfa.setRow(1, m1);
190: Ralfa.setRow(2, m2);
191:
192: return Ralfa;
193: }
194:
195: /**
196: * Generates rotation matrix around Y axis.
197: *
198: * @return rotation Matrix.
199: */
200: protected GeneralMatrix getRbeta() {
201: GeneralMatrix Rbeta = new GeneralMatrix(3, 3);
202: double[] m0 = { Math.cos(beta), 0, -Math.sin(beta) };
203: double[] m1 = { 0, 1, 0 };
204: double[] m2 = { Math.sin(beta), 0, Math.cos(beta) };
205: Rbeta.setRow(0, m0);
206: Rbeta.setRow(1, m1);
207: Rbeta.setRow(2, m2);
208:
209: return Rbeta;
210: }
211:
212: /**
213: * Generates rotation matrix around Z axis.
214: *
215: * @return rotation Matrix.
216: */
217: protected GeneralMatrix getRgamma() {
218: GeneralMatrix Rgamma = new GeneralMatrix(3, 3);
219: double[] m0 = { Math.cos(gamma), Math.sin(gamma), 0 };
220: double[] m1 = { -Math.sin(gamma), Math.cos(gamma), 0 };
221: double[] m2 = { 0, 0, 1 };
222: Rgamma.setRow(0, m0);
223: Rgamma.setRow(1, m1);
224: Rgamma.setRow(2, m2);
225:
226: return Rgamma;
227: }
228:
229: /**
230: * Generates partial derivative with respect to alfa.
231: *
232: * @return Matrix, that represents partial derivation of rotation Matrix
233: * with respect to alfa.
234: */
235: protected GeneralMatrix getDRalfa() {
236: GeneralMatrix dRa = new GeneralMatrix(3, 3);
237:
238: double[] m0 = { 0, 0, 0 };
239: double[] m1 = { 0, -Math.sin(alfa), Math.cos(alfa) };
240: double[] m2 = { 0, -Math.cos(alfa), -Math.sin(alfa) };
241:
242: dRa.setRow(0, m0);
243: dRa.setRow(1, m1);
244: dRa.setRow(2, m2);
245:
246: dRa.mul(dRa, getRbeta());
247: dRa.mul(dRa, getRgamma());
248:
249: return specialMul(dRa, x);
250: }
251:
252: /**
253: * Generates partial derivative with respect to beta.
254: *
255: * @return Matrix, that represents partial derivation of rotation Matrix
256: * with respect to beta.
257: */
258: protected GeneralMatrix getDRbeta() {
259: //GeneralMatrix dRbeta = new GeneralMatrix(3 * sourcePoints.size(), 1);
260: GeneralMatrix dRb = new GeneralMatrix(3, 3);
261: double[] m0 = { -Math.sin(beta), 0, -Math.cos(beta) };
262: double[] m1 = { 0, 0, 0 };
263: double[] m2 = { Math.cos(beta), 0, -Math.sin(beta) };
264: dRb.setRow(0, m0);
265: dRb.setRow(1, m1);
266: dRb.setRow(2, m2);
267:
268: dRb.mul(getRalfa(), dRb);
269: dRb.mul(dRb, getRgamma());
270:
271: return specialMul(dRb, x);
272: }
273:
274: /**
275: * Generates partial derivative with respect to gamma.
276: *
277: * @return Matrix, that represents partial derivation of rotation Matrix
278: * with respect to gamma.
279: */
280: protected GeneralMatrix getDRgamma() {
281: // GeneralMatrix dRgamma = new GeneralMatrix(3 * sourcePoints.size(), 1);
282: GeneralMatrix dRg = new GeneralMatrix(3, 3);
283: GeneralMatrix pom = new GeneralMatrix(3, 3);
284: double[] m0 = { -Math.sin(gamma), Math.cos(gamma), 0 };
285: double[] m1 = { -Math.cos(gamma), -Math.sin(gamma), 0 };
286: double[] m2 = { 0, 0, 0 };
287: dRg.setRow(0, m0);
288: dRg.setRow(1, m1);
289: dRg.setRow(2, m2);
290:
291: pom.mul(getRalfa(), getRbeta());
292: dRg.mul(pom, dRg);
293:
294: return specialMul(dRg, x);
295: }
296:
297: /**
298: * Generates partial derivative in q (scale factor).
299: *
300: * @return rotation Matrix.
301: */
302: protected GeneralMatrix getDq() {
303: // GeneralMatrix Dq = new GeneralMatrix(3 * sourcePoints.size(), 1);
304: GeneralMatrix R = new GeneralMatrix(3, 3);
305: R.mul(getRalfa(), getRbeta());
306: R.mul(R, getRgamma());
307:
308: return specialMul(R, x);
309: }
310:
311: /**
312: * Calculates the matrix of errors from aproximate values of
313: * prameters.
314: *
315: * @return the l matrix.
316: */
317: protected GeneralMatrix getl() {
318: GeneralMatrix l = new GeneralMatrix(3 * getMappedPositions()
319: .size(), 1);
320: GeneralMatrix R = new GeneralMatrix(3, 3);
321: GeneralMatrix T = new GeneralMatrix(3, 1, new double[] { -dx,
322: -dy, -dz });
323: GeneralMatrix qMatrix = new GeneralMatrix(1, 1,
324: new double[] { q });
325: GeneralMatrix qx = new GeneralMatrix(X.getNumRow(), X
326: .getNumCol());
327: qx.mul(x, qMatrix);
328: R.mul(getRalfa(), getRbeta());
329: R.mul(getRgamma());
330:
331: l.sub(specialMul(R, qx), X);
332: l = specialSub(T, l);
333:
334: return l;
335: }
336:
337: /**
338: * Method for multiplying matrix (3,3) by matrix of coordintes (3
339: * number of coordinates,1)
340: *
341: * @param R ratrix
342: * @param x matrix
343: *
344: * @return matrix
345: */
346: protected GeneralMatrix specialMul(GeneralMatrix R, GeneralMatrix x) {
347: GeneralMatrix dRx = new GeneralMatrix(3 * getMappedPositions()
348: .size(), 1);
349:
350: for (int i = 0; i < x.getNumRow(); i = i + 3) {
351: GeneralMatrix subMatrix = new GeneralMatrix(3, 1);
352: x.copySubMatrix(i, 0, 3, 1, 0, 0, subMatrix);
353: subMatrix.mul(R, subMatrix);
354: subMatrix.copySubMatrix(0, 0, 3, 1, i, 0, dRx);
355: }
356:
357: return dRx;
358: }
359:
360: /**
361: * Method for addition matrix (3,3) with matrix of coordintes (3
362: * number of coordinates,1)
363: *
364: * @param R ratrix
365: * @param x matrix
366: *
367: * @return matrix
368: */
369: private GeneralMatrix specialSub(GeneralMatrix R, GeneralMatrix x) {
370: GeneralMatrix dRx = new GeneralMatrix(3 * getMappedPositions()
371: .size(), 1);
372:
373: for (int i = 0; i < x.getNumRow(); i = i + 3) {
374: GeneralMatrix subMatrix = new GeneralMatrix(3, 1);
375: x.copySubMatrix(i, 0, 3, 1, 0, 0, subMatrix);
376: subMatrix.sub(R, subMatrix);
377: subMatrix.copySubMatrix(0, 0, 3, 1, i, 0, dRx);
378: }
379:
380: return dRx;
381: }
382:
383: /**
384: * Glues the submatrix of derivations into the A matrix.
385: *
386: * @return A mtarix
387: */
388: protected GeneralMatrix getA() {
389: final int size = getMappedPositions().size();
390: GeneralMatrix A = new GeneralMatrix(3 * size, 7);
391: GeneralMatrix DT = new GeneralMatrix(3, 3);
392:
393: // the partial derivative with respect to dx,dy,dz.
394: double[] m0 = { 1, 0, 0 };
395: double[] m1 = { 0, 1, 0 };
396: double[] m2 = { 0, 0, 1 };
397: DT.setRow(0, m0);
398: DT.setRow(1, m1);
399: DT.setRow(2, m2);
400:
401: for (int i = 0; i < A.getNumRow(); i = i + 3) {
402: DT.copySubMatrix(0, 0, 3, 3, i, 0, A);
403: }
404:
405: getDRalfa().copySubMatrix(0, 0, 3 * size, 1, 0, 3, A);
406: getDRbeta().copySubMatrix(0, 0, 3 * size, 1, 0, 4, A);
407: getDRgamma().copySubMatrix(0, 0, 3 * size, 1, 0, 5, A);
408: getDq().copySubMatrix(0, 0, 3 * size, 1, 0, 6, A);
409:
410: return A;
411: }
412:
413: /**
414: * Returns array of doubles of transformation parameters (dx, dy,
415: * dz, ex, ey, ez, scale).
416: *
417: * @return array of doubles of transformation parameters (dx, dy, dz, ex,
418: * ey, ez, scale).
419: */
420: protected double[] getParameters() {
421: return getDxMatrix().getElements()[0];
422: }
423:
424: /**
425: * Method that claculates the parameters by iteration. The
426: * tolarance is set to 1 10<sub>-8</sub> and max �number of steps is set
427: * to 20.
428: *
429: * @return Matrix of parameters (dx, dy, dz, ex, ey, ez, scale).
430: */
431: public GeneralMatrix getDxMatrix() {
432: return getDxMatrix(0.00000001, 20);
433: }
434:
435: /**
436: * Iterates the parameters..
437: *
438: * @param tolerance for iteration steps (the max difference between last
439: * two steps)
440: * @param maxSteps highest number of iteations.
441: *
442: * @return GeneralMatrix of calculated parameters.
443: */
444: private GeneralMatrix getDxMatrix(double tolerance, int maxSteps) {
445: // Matriix of new calculated coefficeients
446: GeneralMatrix xNew = new GeneralMatrix(7, 1);
447:
448: // Matrix of coefficients claculated in previous iteration
449: GeneralMatrix xOld = new GeneralMatrix(7, 1);
450:
451: // diference between each steps of old iteration
452: GeneralMatrix dxMatrix = new GeneralMatrix(7, 1);
453:
454: GeneralMatrix zero = new GeneralMatrix(7, 1);
455: zero.setZero();
456:
457: // i is a number of iterations
458: int i = 0;
459:
460: // cicle for iteration
461: do {
462: xOld.set(new double[] { dx, dy, dz, alfa, beta, gamma, q });
463:
464: GeneralMatrix A = getA();
465: GeneralMatrix l = getl();
466:
467: GeneralMatrix AT = (GeneralMatrix) A.clone();
468: AT.transpose();
469:
470: GeneralMatrix ATA = new GeneralMatrix(7, 7);
471: GeneralMatrix ATl = new GeneralMatrix(7, 1);
472:
473: // dx = A^T * A * A^T * l
474: ATA.mul(AT, A);
475: ATA.invert();
476: ATl.mul(AT, l);
477:
478: dxMatrix.mul(ATA, ATl);
479:
480: // New values of x = dx + previous values
481: xOld.negate();
482: xNew.sub(dxMatrix, xOld);
483:
484: // New values are setup for another iteration
485: dx = xNew.getElement(0, 0);
486: dy = xNew.getElement(1, 0);
487: dz = xNew.getElement(2, 0);
488: alfa = xNew.getElement(3, 0);
489: beta = xNew.getElement(4, 0);
490: gamma = xNew.getElement(5, 0);
491: q = xNew.getElement(6, 0);
492:
493: i++;
494: } while ((!dxMatrix.epsilonEquals(zero, tolerance) & (i < maxSteps)));
495:
496: xNew.transpose();
497:
498: return xNew;
499: }
500:
501: /**
502: * Coverts radians to seconds.
503: *
504: * @param rad Angle in radians
505: *
506: * @return Angle is seconds
507: */
508: private static double radiansToSeconds(double rad) {
509: return (rad * (180 / Math.PI) * (3600));
510: }
511:
512: /**
513: * Returns Bursa Wolf Transformation parameters.
514: *
515: * @param Datum The target datum for this parameters.
516: *
517: * @return parameters the BursaWolfParameters
518: */
519: public BursaWolfParameters getBursaWolfParameters(
520: GeodeticDatum Datum) {
521: BursaWolfParameters parameters = new BursaWolfParameters(Datum);
522: parameters.dx = dx;
523: parameters.dy = dy;
524: parameters.dz = dz;
525: parameters.ex = -radiansToSeconds(alfa);
526: parameters.ey = -radiansToSeconds(beta);
527: parameters.ez = -radiansToSeconds(gamma);
528: parameters.ppm = (q - 1) * 1000000;
529:
530: return parameters;
531: }
532:
533: public void setTargetGeodeticDatum(GeodeticDatum gd) {
534: this .targetDatum = gd;
535: }
536:
537: /**
538: * Returns MathtTransform setup as BursaWolf transformation.
539: *
540: * @return calculated MathTransform
541: *
542: * @throws FactoryException when the size of source and destination point
543: * is not the same or if the number of points is too small to
544: * define such transformation.
545: */
546: protected MathTransform computeMathTransform()
547: throws FactoryException {
548: return new GeocentricTranslation(
549: getBursaWolfParameters(targetDatum));
550: }
551: }
|