org.vesalainen.math.CircleFitter Maven / Gradle / Ivy
/*
* Copyright (C) 2014 Timo Vesalainen
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see Implementation is using equations and Jacobian as described in
* Finding the circle that best fits a set of points
*
Levenberg-Marquardt algorithm is slightly modified from an example from
* EJML
* @author Timo Vesalainen
* @see Finding the circle that best fits a set of points
* @see A Few Methods for Fitting Circles to Data
* @see EJML
* @see org.vesalainen.math.LevenbergMarquardt
*/
public class CircleFitter implements Function, JacobianFactory, Circle, Serializable
{
private static final long serialVersionUID = 1L;
private static final double Epsilon = 1e-10;
private DenseMatrix64F di;
private final DenseMatrix64F initCenter = new DenseMatrix64F(2, 1);
private final double[] centerData = initCenter.data;
private final DenseMatrix64F zero = new DenseMatrix64F(1, 1);
private final LevenbergMarquardt levenbergMarquardt = new LevenbergMarquardt(this, this);
private double radius;
private DenseMatrix64F points;
/**
* Creates a CircleFitter with estimated tempCenter. Estimated tempCenter can by
calculated with method initialCenter
* @see org.vesalainen.math.CircleFitter#initialCenter(org.ejml.data.DenseMatrix64F, org.ejml.data.DenseMatrix64F)
*/
public CircleFitter()
{
}
/**
* Fits points to a circle
* @param points
*/
public Circle fit(Point initPoint, DenseMatrix64F points)
{
initCenter.data[0] = initPoint.getX();
initCenter.data[1] = initPoint.getY();
radius = Double.NaN;
fit(points);
return this;
}
public Circle fit(DenseMatrix64F initPoint, DenseMatrix64F points)
{
initCenter.setReshape(initPoint);
radius = Double.NaN;
fit(points);
return this;
}
/**
* Fits points to a circle
* @param points
*/
public Circle fit(Circle initCircle, DenseMatrix64F points)
{
initCenter.data[0] = initCircle.getX();
initCenter.data[1] = initCircle.getY();
radius = initCircle.getRadius();
fit(points);
return this;
}
private void fit(DenseMatrix64F points)
{
this.points = points;
radius = Double.NaN;
if (zero.numRows != points.numRows)
{
zero.reshape(points.numRows, 1);
}
if (levenbergMarquardt.optimize(initCenter, points, zero))
{
initCenter.set(levenbergMarquardt.getParameters());
}
else
{
throw new IllegalArgumentException("Fit failed");
}
}
/**
* Filters points which are closes to the last estimated tempCenter.
* @param points
* @param center
*/
public static void filterInnerPoints(DenseMatrix64F points, DenseMatrix64F center, int minLeft, double percent)
{
assert points.numCols == 2;
assert center.numCols == 1;
assert center.numRows == 2;
if (percent <= 0 || percent >= 1)
{
throw new IllegalArgumentException("percent "+percent+" is not between 0 & 1");
}
DistComp dc = new DistComp(center.data[0], center.data[1]);
Matrices.sort(points, dc);
int rows = points.numRows;
double[] d = points.data;
double limit = dc.distance(d[0], d[1])*percent;
for (int r=minLeft;r max)
{
if (r < min)
{
r = min;
}
else
{
r = max;
}
double m = dy/dx;
if (Double.isInfinite(m))
{
if (m > 0)
{
pr.data[1] = y0 + r;
}
else
{
pr.data[1] = y0 - r;
}
}
else
{
double x = Math.sqrt((r*r)/(m*m+1));
double y = m*x;
pr.data[0] = x0 + x;
pr.data[1] = y0 + y;
}
}
}
/**
* Calculates mean tempCenter of points
* @param points in
* @param center out
* @return
*/
public static double meanCenter(DenseMatrix64F points, DenseMatrix64F center)
{
assert points.numCols == 2;
assert center.numCols == 1;
assert center.numRows == 2;
center.zero();
int count = points.numRows;
double[] d = points.data;
for (int i=0;i 0)
{
divide(center, count);
DenseMatrix64F di = new DenseMatrix64F(points.numRows, 1);
computeDi(center, points, di);
return elementSum(di) / (double)points.numRows;
}
else
{
return Double.NaN;
}
}
/**
* Calculates an initial estimate for tempCenter.
* @param points
* @param center
* @return
*/
public static double initialCenter(DenseMatrix64F points, DenseMatrix64F center)
{
assert points.numCols == 2;
assert center.numCols == 1;
assert center.numRows == 2;
center.zero();
int count = 0;
int len1 = points.numRows;
int len2 = len1-1;
int len3 = len2-1;
for (int i=0;i 0)
{
divide(center, count);
DenseMatrix64F di = new DenseMatrix64F(points.numRows, 1);
computeDi(center, points, di);
return elementSum(di) / (double)points.numRows;
}
else
{
return Double.NaN;
}
}
private static boolean center(DenseMatrix64F points, int i, int j, int k, DenseMatrix64F center)
{
double xi = points.get(i, 0);
double yi = points.get(i, 1);
double xj = points.get(j, 0);
double yj = points.get(j, 1);
double xk = points.get(k, 0);
double yk = points.get(k, 1);
double det = (xk-xj)*(yj-yi)-(xj-xi)*(yk-yj);
if (Math.abs(det) < Epsilon)
{
return false; // aligned
}
double det2 = 2.0*det;
double xyi2 = xi*xi+yi*yi;
double xyj2 = xj*xj+yj*yj;
double xyk2 = xk*xk+yk*yk;
double x = ((yk-yj)*xyi2+(yi-yk)*xyj2+(yj-yi)*xyk2)/det2;
double y = -((xk-xj)*xyi2+(xi-xk)*xyj2+(xj-xi)*xyk2)/det2;
center.add(0, 0, x);
center.add(1, 0, y);
return true;
}
private void computeDi(DenseMatrix64F center, DenseMatrix64F points)
{
if (di == null)
{
di = new DenseMatrix64F(points.numRows, 1);
}
else
{
if (di.numRows != points.numRows)
{
di.reshape(points.numRows, 1);
}
}
computeDi(center, points, di);
}
private static void computeDi(DenseMatrix64F center, DenseMatrix64F points, DenseMatrix64F di)
{
double xx = center.get(0, 0);
double yy = center.get(1, 0);
for (int row=0;row dp)
{
return -1;
}
else
{
return 0;
}
}
}
private double distance(double xx, double yy)
{
double dx = x-xx;
double dy = y-yy;
return Math.sqrt(dx*dx+dy*dy);
}
}
}