/*-
*******************************************************************************
* 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;
}
}