001/******************************************************************************* 002 * This software is provided as a supplement to the authors' textbooks on digital 003 * image processing published by Springer-Verlag in various languages and editions. 004 * Permission to use and distribute this software is granted under the BSD 2-Clause 005 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause). 006 * Copyright (c) 2006-2023 Wilhelm Burger, Mark J. Burge. All rights reserved. 007 * Visit https://imagingbook.com for additional details. 008 ******************************************************************************/ 009package imagingbook.common.geometry.fitting.ellipse.geometric; 010 011import imagingbook.common.geometry.basic.Pnt2d; 012import imagingbook.common.geometry.ellipse.GeometricEllipse; 013import imagingbook.common.geometry.fitting.ellipse.algebraic.EllipseFitAlgebraic; 014import imagingbook.common.geometry.fitting.ellipse.algebraic.EllipseFitFitzgibbonStable; 015import imagingbook.common.geometry.fitting.ellipse.utils.EllipseSampler; 016import imagingbook.common.ij.DialogUtils.DialogLabel; 017import imagingbook.common.math.Matrix; 018import imagingbook.common.math.PrintPrecision; 019import imagingbook.common.util.ParameterBundle; 020import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory; 021import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum; 022import org.apache.commons.math3.fitting.leastsquares.LeastSquaresProblem; 023import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer; 024import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction; 025import org.apache.commons.math3.linear.Array2DRowRealMatrix; 026import org.apache.commons.math3.linear.ArrayRealVector; 027import org.apache.commons.math3.linear.RealMatrix; 028import org.apache.commons.math3.linear.RealVector; 029import org.apache.commons.math3.optim.SimpleVectorValueChecker; 030import org.apache.commons.math3.util.Pair; 031 032import java.util.ArrayList; 033import java.util.List; 034 035import static imagingbook.common.math.Arithmetic.isZero; 036import static imagingbook.common.math.Arithmetic.sqr; 037import static imagingbook.common.math.Matrix.makeRealMatrix; 038import static imagingbook.common.math.Matrix.subtract; 039import static java.lang.Math.cos; 040import static java.lang.Math.signum; 041import static java.lang.Math.sin; 042import static java.lang.Math.sqrt; 043import static org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory.evaluationChecker; 044 045/** 046 * <p> 047 * "Distance-based" geometric ellipse fitter using iterative minimization with the Levenberg-Marquart method. See Sec. 048 * 11.2.2 of [1] for details. 049 * </p> 050 * <p> 051 * [1] W. Burger, M.J. Burge, <em>Digital Image Processing – An Algorithmic Introduction</em>, 3rd ed, Springer 052 * (2022). 053 * </p> 054 * 055 * @author WB 056 * @version 2022/11/17 057 */ 058public class EllipseGeometricFitDist extends EllipseFitGeometric { 059 060 private final Pnt2d[] pts; 061 private final double tolerance; 062 private final int maxEvaluations; 063 private final int maxIterations; 064 065 private final double[] V; 066 private final double[][] J; 067 068 private final MultivariateJacobianFunction model; 069 private final Optimum solution; 070 private final List<double[]> history = new ArrayList<>(); 071 072 public EllipseGeometricFitDist(Pnt2d[] pts, GeometricEllipse initEllipse, int maxEvaluations, 073 int maxIterations, double tolerance, boolean syntheticDeriv) { 074 this.pts = pts; 075 int n = pts.length; 076 this.V = new double[n]; 077 this.J = new double[n][5]; 078 this.maxEvaluations = maxEvaluations; 079 this.maxIterations = maxIterations; 080 this.tolerance = tolerance; 081 this.model = (syntheticDeriv) ? new SyntheticModel() : new AnalyticModel(); 082 this.solution = solveLM(initEllipse.getParameters()); 083 } 084 085 public EllipseGeometricFitDist(Pnt2d[] pts, GeometricEllipse initEllipse) { 086 this(pts, initEllipse, DefaultMaxIterations, DefaultMaxIterations, DefaultTolerance, false); 087 } 088 089 // -------------------------------------------------------------------------- 090 091 @Override 092 public double[] getParameters() { 093 return solution.getPoint().toArray(); 094 } 095 096 @Override 097 public int getIterations() { 098 return solution.getIterations(); 099 } 100 101 @Override 102 public List<double[]> getHistory() { 103 return this.history; 104 } 105 106 // -------------------------------------------------------------------------- 107 108 private Optimum solveLM(double[] p0) { 109 LeastSquaresProblem problem = LeastSquaresFactory.create( 110 this.model, 111 new ArrayRealVector(pts.length), // zero vector 112 new ArrayRealVector(p0), 113 evaluationChecker(new SimpleVectorValueChecker(tolerance, tolerance)), 114 maxEvaluations, maxIterations); 115 116 LevenbergMarquardtOptimizer optimizer = new LevenbergMarquardtOptimizer(); 117 Optimum sol = optimizer.optimize(problem); 118 return sol; 119 } 120 121 // -------------------------------------------------------------------------- 122 123 class AnalyticModel implements MultivariateJacobianFunction { 124 125 @Override 126 public Pair<RealVector, RealMatrix> value(RealVector point) { 127 final double[] p = point.toArray(); 128 if (RecordHistory) { 129 history.add(p.clone()); 130 } 131 final GeometricEllipse ellipse = new GeometricEllipse(p); 132 final double ra = ellipse.ra; 133 final double rb = ellipse.rb; 134 final double xc = ellipse.xc; 135 final double yc = ellipse.yc; 136 final double theta = ellipse.theta; 137 final double ra2 = sqr(ra); 138 final double rb2 = sqr(rb); 139 final double[] xyc = new double[] {xc, yc}; 140 141 RealMatrix R = makeRealMatrix(2, 2, 142 cos(theta), -sin(theta), 143 sin(theta), cos(theta)); 144 RealMatrix Rt = R.transpose(); 145 146 // calculate values and Jacobian 147 for (int i = 0; i < pts.length; i++) { 148 Pnt2d XY = pts[i]; 149 Pnt2d XYp = ellipse.getClosestPoint(XY); 150 double d = XYp.distance(XY); 151 V[i] = d; 152 153 double[] xy = XY.toDoubleArray(); 154 double x = xy[0], y = xy[1]; 155 double[] uv = Rt.operate(subtract(xy, xyc)); // target point in canon. coordinates 156 double u = uv[0], v = uv[1]; // = (u_i, v_i) 157 158 double[] xyp = XYp.toDoubleArray(); // ellipse point closest to Xi 159 double xp = xyp[0], yp = xyp[1]; // = (\breve{x}_i, \breve{y}_i) 160 161 double[] uvp = Rt.operate(subtract(xyp, xyc)); // closest point in canon. coordinates 162 double up = uvp[0], vp = uvp[1]; // = (\breve{u}_i, \breve{u}_i) 163 164 double gA = signum((u - up) * up / ra2 + (v - vp) * vp / rb2); 165 double gB = sqrt(sqr(up / ra2) + sqr(vp / rb2)); 166 if (isZero(gB)) { 167 throw new RuntimeException("gB is zero!"); 168 } 169 double g = gA / gB; 170 171 J[i][0] = -g * sqr(up) / (ra2 * ra); 172 J[i][1] = -g * sqr(vp) / (rb2 * rb); 173 if (!isZero(d)) { 174 J[i][2] = (xp - x) / d; 175 J[i][3] = (yp - y) / d; 176 J[i][4] = ((y - yp) * (xc - xp) - (x - xp) * (yc - yp)) / d ; 177 } 178 else { 179 J[i][2] = J[i][3] = J[i][4] = 0; 180 } 181 } 182 183 RealVector VV = new ArrayRealVector(V, false); 184 RealMatrix JJ = new Array2DRowRealMatrix(J, false); 185 return new Pair<>(VV, JJ); 186 } 187 } 188 189 // -------------------------------- 190 191 class SyntheticModel implements MultivariateJacobianFunction { 192 193 final double delta = 0.00001; 194 195 @Override 196 public Pair<RealVector, RealMatrix> value(RealVector point) { 197 double[] p = point.toArray(); 198 if (RecordHistory) { 199 history.add(p.clone()); 200 } 201 GeometricEllipse ellipse = new GeometricEllipse(p); // ellipse for current parameter point 202 GeometricEllipse[] dEllipses = new GeometricEllipse[5]; // differentially modified ellipses 203 for (int j = 0; j < 5; j++) { 204 double[] pd = p.clone(); 205 pd[j] = p[j] + delta; 206 dEllipses[j] = new GeometricEllipse(pd); 207 } 208 209 // calculate values and Jacobian 210 for (int i = 0; i < pts.length; i++) { 211 Pnt2d XYi = pts[i]; 212 Pnt2d XYp = ellipse.getClosestPoint(XYi); 213 double di = XYp.distance(XYi); 214 V[i] = di; // value = distance of point to ellipse 215 216 for (int j = 0; j < 5; j++) { 217 Pnt2d XYpj = dEllipses[j].getClosestPoint(XYi); 218 J[i][j] = (XYpj.distance(XYi) - di) / delta; // estimated partial derivative 219 } 220 } 221 222 RealVector VV = new ArrayRealVector(V, false); 223 RealMatrix JJ = new Array2DRowRealMatrix(J, false); 224 return new Pair<>(VV, JJ); 225 } 226 } 227 228 // ------------------------------------------------------------------- 229 230// public static void main(String[] args) { 231// PrintPrecision.set(9); 232// Pnt2d[] points = null; 233// //Pnt2d[] points = CircleMaker.makeTestCircle(XC, YC, R, 100, Angle0, Angle1, SigmaNoise); 234// //Pnt2d[] points = CircleMaker.makeTestCircle(XC, YC, R, 4, Angle0, Angle1, 0.1); 235// //Pnt2d[] points = CircleSampler.makeTestGander(30); 236// //Pnt2d[] points = CircleSampler.make3Points(30); 237// 238// EllipseFitAlgebraic fitA = new EllipseFitFitzgibbonStable(points); 239// 240// GeometricEllipse ellipseA = GeometricEllipse.from(fitA.getEllipse()); 241// System.out.println("ellipseA = " + ellipseA); 242// System.out.println("errorA = " + ellipseA.getMeanSquareError(points)); 243// 244// EllipseGeometricFitDist fitG = new EllipseGeometricFitDist(points, ellipseA); 245// GeometricEllipse ellipseG = fitG.getEllipse(); 246// System.out.println("ellipseG = " + ellipseG); 247// System.out.println("errorG = " + ellipseG.getMeanSquareError(points)); 248// System.out.println("iterations = " + fitG.getIterations()); 249// 250// for (double[] p : fitG.getHistory()) { 251// System.out.println(" " + Matrix.toString(p)); 252// } 253// 254// // check analytic/synthetic Jacobians: 255// GeometricEllipse eTest = new GeometricEllipse (50, -30, 100, 120, 0.25); 256// Pnt2d xi = Pnt2d.from(110, -70); 257// Pnt2d[] pts = {xi}; 258// 259// { 260// EllipseGeometricFitDist fit = new EllipseGeometricFitDist(pts, eTest); 261// MultivariateJacobianFunction model = fit.new AnalyticModel(); 262// Pair<RealVector, RealMatrix> result = model.value(Matrix.makeRealVector(eTest.getParameters())); 263// RealMatrix J = result.getSecond(); 264// System.out.println("J analytic = \n" + Matrix.toString(J.getData())); 265// } 266// 267// { 268// EllipseGeometricFitDist fit = new EllipseGeometricFitDist(pts, eTest); 269// MultivariateJacobianFunction model = fit.new SyntheticModel(); 270// Pair<RealVector, RealMatrix> result = model.value(Matrix.makeRealVector(eTest.getParameters())); 271// RealMatrix J = result.getSecond(); 272// System.out.println("J synthetic = \n" + Matrix.toString(J.getData())); 273// } 274// } 275 276 // ------------------------------------------------------------------- 277 // ------------------------------------------------------------------- 278 279 public static class Parameters implements ParameterBundle<EllipseGeometricFitDist> { 280 281 @DialogLabel("number of points") 282 public int n = 20; 283 284 @DialogLabel("ellipse center (xc)") 285 public double xc = 200; 286 287 @DialogLabel("ellipsecenter (yc)") 288 public double yc = 190; 289 290 @DialogLabel("major axis radius (ra)") 291 public double ra = 170; 292 293 @DialogLabel("minor axis radius (rb)") 294 public double rb = 120; 295 296 @DialogLabel("start angle (deg)") 297 public double angle0 = 0; 298 299 @DialogLabel("stop angle (deg)") 300 public double angle1 = 180; // was Math.PI/4; 301 302 @DialogLabel("ellipse orientation (deg)") 303 public double theta = 45; 304 305 @DialogLabel("x/y noise (sigma)") 306 public double sigma = 5.0; //2.0; 307 }; 308 309 private static Parameters params = new Parameters(); 310 311 312 public static void main(String[] args) { 313 314 System.out.println("*** Testing " + EllipseGeometricFitDist.class.getSimpleName() + " ***"); 315 PrintPrecision.set(9); 316 317 GeometricEllipse realEllipse = new GeometricEllipse(params.ra, params.rb, params.xc, params.yc, 318 Math.toRadians(params.theta)); 319 320 EllipseSampler sampler = new EllipseSampler(realEllipse, 17); 321 322 Pnt2d[] points = sampler.getPoints(params.n, 323 Math.toRadians(params.angle0), Math.toRadians(params.angle1), params.sigma); 324 325// Pnt2d[] points = null; 326 //Pnt2d[] points = CircleMaker.makeTestCircle(XC, YC, R, 100, Angle0, Angle1, SigmaNoise); 327 //Pnt2d[] points = CircleMaker.makeTestCircle(XC, YC, R, 4, Angle0, Angle1, 0.1); 328 //Pnt2d[] points = CircleSampler.makeTestGander(30); 329 //Pnt2d[] points = CircleSampler.make3Points(30); 330 331 EllipseFitAlgebraic fitA = new EllipseFitFitzgibbonStable(points); 332 333 GeometricEllipse ellipseA = new GeometricEllipse(fitA.getEllipse()); 334 System.out.println("ellipseA = " + ellipseA); 335 System.out.println("errorA = " + ellipseA.getMeanSquareError(points)); 336 337 EllipseGeometricFitDist fitG = new EllipseGeometricFitDist(points, ellipseA); 338 GeometricEllipse ellipseG = fitG.getEllipse(); 339 System.out.println("ellipseG = " + ellipseG); 340 System.out.println("errorG = " + ellipseG.getMeanSquareError(points)); 341 System.out.println("iterations = " + fitG.getIterations()); 342 343 for (double[] p : fitG.getHistory()) { 344 System.out.println(" " + Matrix.toString(p)); 345 } 346 347 // check analytic/synthetic Jacobians: 348 GeometricEllipse eTest = new GeometricEllipse (100, 120, 50, -30, 0.25); 349 Pnt2d xi = Pnt2d.from(110, -70); 350 Pnt2d[] pts = {xi}; 351 352 { 353 EllipseGeometricFitDist fit = new EllipseGeometricFitDist(pts, ellipseA); 354 MultivariateJacobianFunction model = fit.new AnalyticModel(); 355 Pair<RealVector, RealMatrix> result = model.value(Matrix.makeRealVector(eTest.getParameters())); 356 RealMatrix J = result.getSecond(); 357 System.out.println("J analytic = \n" + Matrix.toString(J.getData())); 358 } 359 360 { 361 EllipseGeometricFitDist fit = new EllipseGeometricFitDist(pts, ellipseA); 362 MultivariateJacobianFunction model = fit.new SyntheticModel(); 363 Pair<RealVector, RealMatrix> result = model.value(Matrix.makeRealVector(eTest.getParameters())); 364 RealMatrix J = result.getSecond(); 365 System.out.println("J synthetic = \n" + Matrix.toString(J.getData())); 366 } 367 } 368 369}