org.eclipse.dawnsci.analysis.dataset.roi.fitting.EllipseCoordinatesFunction.java Source code

Java tutorial

Introduction

Here is the source code for org.eclipse.dawnsci.analysis.dataset.roi.fitting.EllipseCoordinatesFunction.java

Source

/*-
 *******************************************************************************
 * 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.analysis.solvers.BrentSolver;
import org.apache.commons.math3.exception.DimensionMismatchException;
import org.apache.commons.math3.exception.MathIllegalArgumentException;
import org.apache.commons.math3.exception.TooManyEvaluationsException;
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.ArrayRealVector;
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.LinearAlgebra;
import org.eclipse.january.dataset.Maths;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import Jama.Matrix;

/**
 * This function returns the coordinates (interleaved) for the points specified by the
 * geometric parameters and an array of angles
 */
class EllipseCoordinatesFunction implements IConicSectionFitFunction, Serializable {
    private static final int PARAMETERS = EllipseFitter.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;

    AngleDerivativeFunction angleDerivative = new AngleDerivativeFunction();
    static BrentSolver solver = new BrentSolver();

    public EllipseCoordinatesFunction(IDataset x, IDataset y) {
        setPoints(x, y);
    }

    //   public UnivariateRealFunction getAngleDerivative() {
    //      return angleDerivative;
    //   }

    @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][3] = 1;
            i++;
            j[i][4] = 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 ellipse 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 a = initParameters[0];
        final double b = initParameters[1];
        final double alpha = initParameters[2];
        final double x = initParameters[3];
        final double y = initParameters[4];
        final double twopi = 2 * Math.PI;
        angleDerivative.setRadii(a, b);
        angleDerivative.setAngle(alpha);

        // work out the angle values for the closest points on ellipse
        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;

            angleDerivative.setCoordinate(Xc, Yc);
            try {
                // find quadrant to use
                double pa = Math.atan2(Yc, Xc);
                if (pa < 0)
                    pa += twopi;
                pa -= alpha;
                final double end;
                final double halfpi = 0.5 * Math.PI;
                pa /= halfpi;
                end = Math.ceil(pa) * halfpi;
                final double angle = solver.solve(100, angleDerivative, end - halfpi, end);
                init[i++] = angle;
            } catch (TooManyEvaluationsException e) {
                throw new IllegalArgumentException("Problem with solver converging as iterations exceed limit");
            } catch (MathIllegalArgumentException e) {
                // cannot happen
            }
        }
        return new InitialGuess(init);
    }

    @Override
    public double[] value(double[] p) {
        final double[] values = v.getData();
        final double a = p[0];
        final double b = p[1];
        final double cosa = Math.cos(p[2]);
        final double sina = Math.sin(p[2]);
        final double x = p[3];
        final double y = p[4];

        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;
            values[2 * i] = x + a * cosa * cost - b * sina * sint;
            values[2 * i + 1] = y + a * sina * cost + b * cosa * 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 a = p[0];
        final double b = p[1];
        final double cosa = Math.cos(p[2]);
        final double sina = Math.sin(p[2]);
        final double x = p[3];
        final double y = p[4];

        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 + a * cosa * cost - b * sina * sint - X.getElementDoubleAbs(i);
            double py = y + a * sina * cost + b * cosa * sint - Y.getElementDoubleAbs(i);
            values[i] = px * px + py * py;
        }

        return v;
    }

    private void calculateJacobian(double[] p) {
        final double ta = 2 * p[0];
        final double aa = p[0] * p[0];
        final double tb = 2 * p[1];
        final double bb = p[1] * p[1];
        final double cosa = Math.cos(p[2]);
        final double sina = Math.sin(p[2]);

        for (int i = 0; i < n; i++) {
            final double cost = ca[i];
            final double sint = sa[i];
            final double cc = cosa * cost;
            final double cs = cosa * sint;
            final double sc = sina * cost;
            final double ss = sina * sint;

            final int ti = 2 * i;
            final int tj = 2 * i + 1;
            j[ti][0] = ta * cc;
            j[ti][1] = -tb * ss;
            j[ti][2] = -aa * sc - bb * cs;
            j[ti][PARAMETERS + i] = -aa * cs - bb * sc;

            j[tj][0] = ta * sc;
            j[tj][1] = tb * cs;
            j[tj][2] = aa * cc - bb * ss;
            j[tj][PARAMETERS + i] = -aa * ss + bb * cc;
        }
    }

    @Override
    public MultivariateMatrixFunction jacobian() {
        return new MultivariateMatrixFunction() {
            @Override
            public double[][] value(double[] p) throws IllegalArgumentException {
                calculateJacobian(p);
                return j;
            }
        };
    }
}

/**
 * Fit an ellipse whose geometric parameters are
 *  major, minor semi-axes, angle of major axis, centre coordinates
 */
public class EllipseFitter implements IConicSectionFitter, Serializable {
    /**
     * Setup the logging facilities
     */
    private static final Logger logger = LoggerFactory.getLogger(EllipseFitter.class);

    private double[] parameters;
    private double residual;

    private EllipseCoordinatesFunction fitFunction;

    final static int PARAMETERS = 5;
    private static final int MAX_EVALUATIONS = Integer.MAX_VALUE / 1024;

    public EllipseFitter() {
        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 EllipseCoordinatesFunction(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 (init == null)
            init = quickfit(x, y);
        else if (init.length < PARAMETERS)
            throw new IllegalArgumentException("Need " + PARAMETERS + " parameters");

        EllipseCoordinatesFunction f = (EllipseCoordinatesFunction) 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("Ellipse fit: rms = {}, x^2 = {}", residual, opt.getChiSquare());
        } catch (DimensionMismatchException e) {
            // cannot happen
        } catch (IllegalArgumentException e) {
            // should not happen!
        } catch (TooManyEvaluationsException e) {
            throw new IllegalArgumentException("Problem with optimizer converging");
        }
    }

    @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 method in "Numerically stable direct least squares fitting of ellipses"
     * by R. Halir and J. Flusser, Proceedings of the 6th International Conference in Central Europe
     * on Computer Graphics and Visualization. WSCG '98. CZ, Pilsen 1998, pp 125-132. 
     * <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);
        final Dataset xx = Maths.square(x);
        final Dataset yy = Maths.square(y);
        final Dataset xxx = Maths.multiply(xx, x);
        final Dataset yyy = Maths.multiply(yy, y);
        final Dataset xy = Maths.multiply(x, y);

        Matrix S1 = new Matrix(3, 3);
        S1.set(0, 0, LinearAlgebra.dotProduct(xx, xx).getDouble());
        S1.set(0, 1, LinearAlgebra.dotProduct(xxx, y).getDouble());
        S1.set(0, 2, LinearAlgebra.dotProduct(xx, yy).getDouble());

        S1.set(1, 0, S1.get(0, 1));
        S1.set(1, 1, S1.get(0, 2));
        S1.set(1, 2, LinearAlgebra.dotProduct(x, yyy).getDouble());

        S1.set(2, 0, S1.get(0, 2));
        S1.set(2, 1, S1.get(1, 2));
        S1.set(2, 2, LinearAlgebra.dotProduct(yy, yy).getDouble());

        Matrix S2 = new Matrix(3, 3);
        S2.set(0, 0, ((Number) xxx.sum()).doubleValue());
        S2.set(0, 1, LinearAlgebra.dotProduct(xx, y).getDouble());
        S2.set(0, 2, ((Number) xx.sum()).doubleValue());

        S2.set(1, 0, S2.get(0, 1));
        S2.set(1, 1, LinearAlgebra.dotProduct(x, yy).getDouble());
        S2.set(1, 2, ((Number) xy.sum()).doubleValue());

        S2.set(2, 0, S2.get(1, 1));
        S2.set(2, 1, ((Number) yyy.sum()).doubleValue());
        S2.set(2, 2, ((Number) yy.sum()).doubleValue());

        Matrix S3 = new Matrix(3, 3);
        S3.set(0, 0, S2.get(0, 2));
        S3.set(0, 1, S2.get(1, 2));
        S3.set(0, 2, ((Number) x.sum()).doubleValue());

        S3.set(1, 0, S3.get(0, 1));
        S3.set(1, 1, S2.get(2, 2));
        S3.set(1, 2, ((Number) y.sum()).doubleValue());

        S3.set(2, 0, S3.get(0, 2));
        S3.set(2, 1, S3.get(1, 2));
        S3.set(2, 2, x.getSize());

        Matrix T = S3.solve(S2.transpose()).uminus();

        Matrix M = S1.plus(S2.times(T));

        Matrix Cinv = new Matrix(new double[] { 0, 0, 0.5, 0, -1.0, 0, 0.5, 0, 0 }, 3);
        Matrix Mp = Cinv.times(M);

        //      System.err.println("M " + Arrays.toString(Mp.getRowPackedCopy()));
        Matrix V = Mp.eig().getV();
        //      System.err.println("V " + Arrays.toString(V.getRowPackedCopy()));
        double[][] mv = V.getArray();
        ArrayRealVector v1 = new ArrayRealVector(mv[0]);
        ArrayRealVector v2 = new ArrayRealVector(mv[1]);
        ArrayRealVector v3 = new ArrayRealVector(mv[2]);

        v1.mapMultiplyToSelf(4);

        ArrayRealVector v = v1.ebeMultiply(v3).subtract(v2.ebeMultiply(v2));
        double[] varray = v.getDataRef();
        int i = 0;
        for (; i < 3; i++) {
            if (varray[i] > 0)
                break;
        }
        if (i == 3) {
            throw new IllegalArgumentException("Could not find solution that satifies constraint");
        }

        v = new ArrayRealVector(new double[] { mv[0][i], mv[1][i], mv[2][i] });
        varray = v.getDataRef();
        final double ca = varray[0];
        final double cb = varray[1];
        final double cc = varray[2];
        Array2DRowRealMatrix mt = new Array2DRowRealMatrix(T.getArray(), false);
        varray = mt.operate(varray);
        final double cd = varray[0];
        final double ce = varray[1];
        final double cf = varray[2];

        //      System.err.println(String.format("Algebraic: %g, %g, %g, %g, %g, %g", ca, cb, cc, cd, ce, cf));
        final double disc = cb * cb - 4. * ca * cc;
        if (disc >= 0) {
            throw new IllegalArgumentException("Solution is not an ellipse");
        }

        if (cb == 0) {
            throw new IllegalArgumentException("Solution is a circle");
        }

        double[] qparameters = new double[PARAMETERS];
        qparameters[3] = (2. * cc * cd - cb * ce) / disc;
        qparameters[4] = (2. * ca * ce - cb * cd) / disc;

        final double sqrt = Math.sqrt((ca - cc) * (ca - cc) + cb * cb);
        qparameters[0] = -2. * (ca * ce * ce + cc * cd * cd + cf * cb * cb - cb * cd * ce - 4. * ca * cc * cf)
                / disc;
        qparameters[1] = qparameters[0] / (ca + cc + sqrt);
        qparameters[0] /= (ca + cc - sqrt);
        qparameters[0] = Math.sqrt(qparameters[0]);
        qparameters[1] = Math.sqrt(qparameters[1]);
        if (cb == 0) {
            qparameters[2] = 0.;
        } else {
            qparameters[2] = 0.5 * Math.atan2(cb, ca - cc);
        }
        if (qparameters[0] < qparameters[1]) {
            final double t = qparameters[0];
            qparameters[0] = qparameters[1];
            qparameters[1] = t;
        } else {
            qparameters[2] += Math.PI * 0.5;
        }

        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 ca = Math.cos(geometricParameters[2]);
        final double sa = Math.sin(geometricParameters[2]);
        final IndexIterator it = angles.getIterator();

        int i = 0;

        while (it.hasNext()) {
            final double t = angles.getElementDoubleAbs(it.index);
            final double ct = Math.cos(t);
            final double st = Math.sin(t);
            x.setAbs(i,
                    geometricParameters[3] + geometricParameters[0] * ca * ct - geometricParameters[1] * sa * st);
            y.setAbs(i,
                    geometricParameters[4] + geometricParameters[0] * sa * ct + geometricParameters[1] * ca * st);
            i++;
        }
        return coords;
    }
}