1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * The ASF licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.apache.commons.math4.legacy.fitting.leastsquares;
18
19 import java.util.ArrayList;
20
21 import org.apache.commons.math4.legacy.analysis.MultivariateMatrixFunction;
22 import org.apache.commons.math4.legacy.analysis.MultivariateVectorFunction;
23 import org.apache.commons.math4.core.jdkmath.JdkMath;
24
25 /**
26 * Class that models a circle.
27 * The parameters of problem are:
28 * <ul>
29 * <li>the x-coordinate of the circle center,</li>
30 * <li>the y-coordinate of the circle center,</li>
31 * <li>the radius of the circle.</li>
32 * </ul>
33 * The model functions are:
34 * <ul>
35 * <li>for each triplet (cx, cy, r), the (x, y) coordinates of a point on the
36 * corresponding circle.</li>
37 * </ul>
38 */
39 class CircleProblem {
40 /** Cloud of points assumed to be fitted by a circle. */
41 private final ArrayList<double[]> points;
42 /** Error on the x-coordinate of the points. */
43 private final double xSigma;
44 /** Error on the y-coordinate of the points. */
45 private final double ySigma;
46 /** Number of points on the circumference (when searching which
47 model point is closest to a given "observation". */
48 private final int resolution;
49
50 /**
51 * @param xError Assumed error for the x-coordinate of the circle points.
52 * @param yError Assumed error for the y-coordinate of the circle points.
53 * @param searchResolution Number of points to try when searching the one
54 * that is closest to a given "observed" point.
55 */
56 CircleProblem(double xError,
57 double yError,
58 int searchResolution) {
59 points = new ArrayList<>();
60 xSigma = xError;
61 ySigma = yError;
62 resolution = searchResolution;
63 }
64
65 /**
66 * @param xError Assumed error for the x-coordinate of the circle points.
67 * @param yError Assumed error for the y-coordinate of the circle points.
68 */
69 CircleProblem(double xError,
70 double yError) {
71 this(xError, yError, 500);
72 }
73
74 public void addPoint(double[] p) {
75 points.add(p);
76 }
77
78 public double[] target() {
79 final double[] t = new double[points.size() * 2];
80 for (int i = 0; i < points.size(); i++) {
81 final double[] p = points.get(i);
82 final int index = i * 2;
83 t[index] = p[0];
84 t[index + 1] = p[1];
85 }
86
87 return t;
88 }
89
90 public double[] weight() {
91 final double wX = 1 / (xSigma * xSigma);
92 final double wY = 1 / (ySigma * ySigma);
93 final double[] w = new double[points.size() * 2];
94 for (int i = 0; i < points.size(); i++) {
95 final int index = i * 2;
96 w[index] = wX;
97 w[index + 1] = wY;
98 }
99
100 return w;
101 }
102
103 public MultivariateVectorFunction getModelFunction() {
104 return new MultivariateVectorFunction() {
105 @Override
106 public double[] value(double[] params) {
107 final double cx = params[0];
108 final double cy = params[1];
109 final double r = params[2];
110
111 final double[] model = new double[points.size() * 2];
112
113 final double twopi = 2 * Math.PI;
114 final double deltaTheta = twopi / resolution;
115 for (int i = 0; i < points.size(); i++) {
116 final double[] p = points.get(i);
117 final double px = p[0];
118 final double py = p[1];
119
120 double bestX = 0;
121 double bestY = 0;
122 double dMin = Double.POSITIVE_INFINITY;
123
124 // Find the angle for which the circle passes closest to the
125 // current point (using a resolution of 100 points along the
126 // circumference).
127 for (double theta = 0; theta <= twopi; theta += deltaTheta) {
128 final double currentX = cx + r * JdkMath.cos(theta);
129 final double currentY = cy + r * JdkMath.sin(theta);
130 final double dX = currentX - px;
131 final double dY = currentY - py;
132 final double d = dX * dX + dY * dY;
133 if (d < dMin) {
134 dMin = d;
135 bestX = currentX;
136 bestY = currentY;
137 }
138 }
139
140 final int index = i * 2;
141 model[index] = bestX;
142 model[index + 1] = bestY;
143 }
144
145 return model;
146 }
147 };
148 }
149
150 public MultivariateMatrixFunction getModelFunctionJacobian() {
151 return new MultivariateMatrixFunction() {
152 @Override
153 public double[][] value(double[] point) {
154 return jacobian(point);
155 }
156 };
157 }
158
159 private double[][] jacobian(double[] params) {
160 final double[][] jacobian = new double[points.size() * 2][3];
161
162 for (int i = 0; i < points.size(); i++) {
163 final int index = i * 2;
164 // Partial derivative wrt x-coordinate of center.
165 jacobian[index][0] = 1;
166 jacobian[index + 1][0] = 0;
167 // Partial derivative wrt y-coordinate of center.
168 jacobian[index][1] = 0;
169 jacobian[index + 1][1] = 1;
170 // Partial derivative wrt radius.
171 final double[] p = points.get(i);
172 jacobian[index][2] = (p[0] - params[0]) / params[2];
173 jacobian[index + 1][2] = (p[1] - params[1]) / params[2];
174 }
175
176 return jacobian;
177 }
178 }