Java tutorial
/*- ******************************************************************************* * Copyright (c) 2011, 2014 Diamond Light Source Ltd. * All rights reserved. This program and the accompanying materials * are made available under the terms of the Eclipse Public License v1.0 * which accompanies this distribution, and is available at * http://www.eclipse.org/legal/epl-v10.html * * Contributors: * Peter Chang - initial API and implementation and/or initial documentation *******************************************************************************/ package org.eclipse.dawnsci.analysis.dataset.roi.fitting; import java.io.Serializable; import java.util.Arrays; import org.apache.commons.math3.analysis.MultivariateMatrixFunction; import org.apache.commons.math3.exception.DimensionMismatchException; import org.apache.commons.math3.exception.TooManyEvaluationsException; import org.apache.commons.math3.optim.InitialGuess; import org.apache.commons.math3.optim.MaxEval; import org.apache.commons.math3.optim.PointVectorValuePair; import org.apache.commons.math3.optim.nonlinear.vector.ModelFunction; import org.apache.commons.math3.optim.nonlinear.vector.ModelFunctionJacobian; import org.apache.commons.math3.optim.nonlinear.vector.Target; import org.apache.commons.math3.optim.nonlinear.vector.Weight; import org.apache.commons.math3.optim.nonlinear.vector.jacobian.LevenbergMarquardtOptimizer; import org.eclipse.dawnsci.analysis.api.fitting.IConicSectionFitFunction; import org.eclipse.dawnsci.analysis.api.fitting.IConicSectionFitter; import org.eclipse.january.dataset.Dataset; import org.eclipse.january.dataset.DatasetFactory; import org.eclipse.january.dataset.DatasetUtils; import org.eclipse.january.dataset.DoubleDataset; import org.eclipse.january.dataset.IDataset; import org.eclipse.january.dataset.IndexIterator; import org.eclipse.january.dataset.Maths; import org.slf4j.Logger; import org.slf4j.LoggerFactory; import Jama.EigenvalueDecomposition; import Jama.Matrix; import Jama.SingularValueDecomposition; /** * This function returns the coordinates (interleaved) for the points specified by the * geometric parameters and an array of angles */ class CircleCoordinatesFunction implements IConicSectionFitFunction, Serializable { private static final int PARAMETERS = CircleFitter.PARAMETERS; private Dataset X; private Dataset Y; private DoubleDataset v; private double[][] j; private int n; // number of points private int m; // number of equations private double[] ca; private double[] sa; public CircleCoordinatesFunction(IDataset x, IDataset y) { setPoints(x, y); } @Override public void setPoints(IDataset x, IDataset y) { X = DatasetUtils.convertToDataset(x); Y = DatasetUtils.convertToDataset(y); n = X.getSize(); m = 2 * n; v = DatasetFactory.zeros(DoubleDataset.class, m); j = new double[m][PARAMETERS + n]; for (int i = 0; i < m; i++) { j[i][1] = 1; i++; j[i][2] = 1; } ca = new double[n]; sa = new double[n]; } @Override public Target getTarget() { double[] target = new double[m]; final IndexIterator itx = X.getIterator(); final IndexIterator ity = Y.getIterator(); int i = 0; while (itx.hasNext() && ity.hasNext()) { target[i++] = X.getElementDoubleAbs(itx.index); target[i++] = Y.getElementDoubleAbs(ity.index); } return new Target(target); } @Override public Weight getWeight() { double[] weight = new double[m]; Arrays.fill(weight, 1.0); return new Weight(weight); } /** * Calculate angles of closest points on circle to targets * @param initParameters geometric parameters * @return array of all initial parameters */ InitialGuess calcAllInitValues(double[] initParameters) { double[] init = new double[n + PARAMETERS]; for (int i = 0; i < initParameters.length; i++) { init[i] = initParameters[i]; } final double x = initParameters[1]; final double y = initParameters[2]; // work out the angle values for the closest points on circle final IndexIterator itx = X.getIterator(); final IndexIterator ity = Y.getIterator(); int i = PARAMETERS; while (itx.hasNext() && ity.hasNext()) { final double Xc = X.getElementDoubleAbs(itx.index) - x; final double Yc = Y.getElementDoubleAbs(ity.index) - y; init[i++] = Math.atan2(Yc, Xc); } return new InitialGuess(init); } @Override public double[] value(double[] p) { final double[] values = v.getData(); final double r = p[0]; final double x = p[1]; final double y = p[2]; for (int i = 0; i < n; i++) { final double t = p[i + PARAMETERS]; final double cost = Math.cos(t); final double sint = Math.sin(t); ca[i] = cost; sa[i] = sint; final int ti = 2 * i; values[ti] = x + r * cost; values[ti + 1] = y + r * sint; } return values; } @Override public Dataset calcDistanceSquared(double[] parameters) throws IllegalArgumentException { final double[] p = calcAllInitValues(parameters).getInitialGuess(); final DoubleDataset v = DatasetFactory.zeros(DoubleDataset.class, n); final double[] values = v.getData(); final double r = p[0]; final double x = p[1]; final double y = p[2]; for (int i = 0; i < n; i++) { final double t = p[i + PARAMETERS]; final double cost = Math.cos(t); final double sint = Math.sin(t); double px = x + r * cost - X.getElementDoubleAbs(i); double py = y + r * sint - Y.getElementDoubleAbs(i); values[i] = px * px + py * py; } return v; } private void calculateJacobian(double[] p) { final double r = p[0]; for (int i = 0; i < n; i++) { final double ct = ca[i]; final double st = sa[i]; final int ti = 2 * i; final int tj = ti + 1; j[ti][0] = ct; j[ti][PARAMETERS + i] = -r * st; j[tj][0] = st; j[tj][PARAMETERS + i] = r * ct; } } @Override public MultivariateMatrixFunction jacobian() { return new MultivariateMatrixFunction() { @Override public double[][] value(double[] p) throws IllegalArgumentException { calculateJacobian(p); return j; } }; } } /** * Fit a circle whose geometric parameters are * radius, centre coordinates */ public class CircleFitter implements IConicSectionFitter, Serializable { /** * Setup the logging facilities */ private static final Logger logger = LoggerFactory.getLogger(CircleFitter.class); private double[] parameters; private CircleCoordinatesFunction fitFunction; private double residual; final static int PARAMETERS = 3; private static final int MAX_EVALUATIONS = Integer.MAX_VALUE / 1024; public CircleFitter() { parameters = new double[PARAMETERS]; } @Override public double[] getParameters() { return parameters; } @Override public IConicSectionFitFunction getFitFunction(IDataset x, IDataset y) { if (fitFunction == null) { if (x == null || y == null) throw new IllegalArgumentException("Fitter uninitialized so coordinate datasets are needed"); fitFunction = new CircleCoordinatesFunction(x, y); } else if (x != null && y != null) { fitFunction.setPoints(x, y); } return fitFunction; } @Override public double getRMS() { return residual; } @Override public void geometricFit(IDataset x, IDataset y, double[] init) { residual = Double.NaN; if (x.getSize() < PARAMETERS || y.getSize() < PARAMETERS) { throw new IllegalArgumentException("Need " + PARAMETERS + " or more points"); } if (x.getSize() == PARAMETERS) { init = quickfit(x, y); for (int i = 0; i < PARAMETERS; i++) parameters[i] = init[i]; residual = 0; return; } if (init == null) init = quickfit(x, y); else if (init.length < PARAMETERS) throw new IllegalArgumentException("Need " + PARAMETERS + " parameters"); CircleCoordinatesFunction f = (CircleCoordinatesFunction) getFitFunction(x, y); LevenbergMarquardtOptimizer opt = new LevenbergMarquardtOptimizer(); try { PointVectorValuePair result = opt.optimize(new ModelFunction(f), new ModelFunctionJacobian(f.jacobian()), f.getTarget(), f.getWeight(), f.calcAllInitValues(init), new MaxEval(MAX_EVALUATIONS)); double[] point = result.getPointRef(); for (int i = 0; i < PARAMETERS; i++) parameters[i] = point[i]; residual = opt.getRMS(); logger.trace("Circle fit: rms = {}, x^2 = {}", opt.getRMS(), opt.getChiSquare()); } catch (DimensionMismatchException e) { // cannot happen } catch (IllegalArgumentException e) { // should not happen! } catch (TooManyEvaluationsException e) { throw new IllegalArgumentException("Problem with optimizer converging"); } } /** * Fit points given by x, y datasets to a circle. * @param x * @param y */ @Override public void algebraicFit(IDataset x, IDataset y) { residual = Double.NaN; if (x.getSize() < PARAMETERS || y.getSize() < PARAMETERS) { throw new IllegalArgumentException("Need " + PARAMETERS + " or more points"); } double[] quick = quickfit(x, y); IConicSectionFitFunction f = getFitFunction(x, y); residual = Math.sqrt((Double) f.calcDistanceSquared(quick).mean()); for (int i = 0; i < PARAMETERS; i++) parameters[i] = quick[i]; } /** * least-squares using algebraic cost function * <p> * This uses the Pratt method as mentioned in "Error analysis for circle fitting algorithms" * by A. Al-Sharadqah and N. Chernov, Electronic Journal of Statistics, v3, pp886-991 (2009) * <p> * @param ix * @param iy * @return geometric parameters */ private static double[] quickfit(IDataset ix, IDataset iy) { Dataset x = DatasetUtils.convertToDataset(ix); Dataset y = DatasetUtils.convertToDataset(iy); double mx = (Double) x.mean(); double my = (Double) y.mean(); x = Maths.subtract(x.cast(Dataset.FLOAT64), mx); y = Maths.subtract(y.cast(Dataset.FLOAT64), my); final DoubleDataset z = (DoubleDataset) Maths.square(x).iadd(Maths.square(y)); final DoubleDataset o = DatasetFactory.ones(DoubleDataset.class, x.getShape()); double ca, cd, ce, cf; if (x.getSize() == PARAMETERS) { // exact case double[][] mz = { (double[]) x.getBuffer(), (double[]) y.getBuffer(), o.getData() }; Matrix Z = new Matrix(mz); Matrix V = new Matrix(z.getData(), 1); try { V = V.times(Z.inverse()); } catch (RuntimeException e) { throw new IllegalArgumentException("Given points are collinear"); } ca = 1; cd = -V.get(0, 0); ce = -V.get(0, 1); cf = -V.get(0, 2); } else { double[][] mz = { z.getData(), (double[]) x.getBuffer(), (double[]) y.getBuffer(), o.getData() }; Matrix Z = new Matrix(mz); SingularValueDecomposition svd = Z.transpose().svd(); Matrix S = svd.getS(); // System.err.println("S:"); // S.print(12, 6); Matrix V = svd.getV(); // System.err.println("V:"); // V.print(12, 6); if (S.get(3, 3) < S.get(0, 0) * 1e-12) { ca = V.get(0, 3); cd = V.get(1, 3); ce = V.get(2, 3); cf = V.get(3, 3); } else { Matrix W = V.times(S); // System.err.println("W:"); // W.print(12, 6); Matrix Cinv = new Matrix(new double[] { 0, 0, 0, -0.5, 0, 1, 0, 0, 0, 0, 1, 0, -0.5, 0, 0, 0 }, 4); Matrix T = W.transpose().times(Cinv.times(W)); // System.err.println("T:"); // T.print(12, 6); EigenvalueDecomposition decomp = T.eig(); double[] e = decomp.getRealEigenvalues(); // System.err.println("Eigenvalues: " + Arrays.toString(e)); // find minimal positive eigenvalue double emin = Double.POSITIVE_INFINITY; int j = 0; for (int i = 0; i < 4; i++) { double ei = e[i]; if (ei > 0 && ei < emin) { emin = ei; j = i; } S.set(i, i, 1. / S.get(i, i)); } Matrix A = decomp.getV(); A = V.times(S).times(A.getMatrix(0, 3, j, j)); // A.print(12, 6); ca = A.get(0, 0); cd = A.get(1, 0); ce = A.get(2, 0); cf = A.get(3, 0); } } final double disc = cd * cd + ce * ce - 4. * ca * cf; // System.err.println(String.format("Algebraic: %g, %g, %g, %g (%g)", ca, cd, ce, cf, disc)); if (disc < 0) { throw new IllegalArgumentException("No solution!"); } double[] qparameters = new double[PARAMETERS]; double f = 0.5 / ca; qparameters[0] = Math.abs(f) * Math.sqrt(disc); qparameters[1] = -f * cd + mx; qparameters[2] = -f * ce + my; // System.err.println(String.format("Algebraic: %g, %g, %g", qparameters[0], qparameters[1], qparameters[2])); return qparameters; } /** * Compute coordinates from an array of angles * @param angles * @param geometricParameters * @return x and y datasets */ public static Dataset[] generateCoordinates(Dataset angles, final double[] geometricParameters) { if (geometricParameters.length != PARAMETERS) throw new IllegalArgumentException("Need " + PARAMETERS + " parameters"); Dataset[] coords = new Dataset[2]; DoubleDataset x = DatasetFactory.zeros(DoubleDataset.class, angles.getShape()); DoubleDataset y = DatasetFactory.zeros(DoubleDataset.class, angles.getShape()); coords[0] = x; coords[1] = y; final double r = geometricParameters[0]; final double cx = geometricParameters[1]; final double cy = geometricParameters[2]; final IndexIterator it = angles.getIterator(); int i = 0; while (it.hasNext()) { final double t = angles.getElementDoubleAbs(it.index); x.setAbs(i, cx + r * Math.cos(t)); y.setAbs(i, cy + r * Math.sin(t)); i++; } return coords; } }