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