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 * https://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
18 /*
19 * This is not the original file distributed by the Apache Software Foundation
20 * It has been modified by the Hipparchus project
21 */
22 package org.hipparchus.optim.nonlinear.vector.leastsquares;
23
24 import java.util.ArrayList;
25
26 import org.hipparchus.analysis.MultivariateMatrixFunction;
27 import org.hipparchus.analysis.MultivariateVectorFunction;
28 import org.hipparchus.util.FastMath;
29 import org.hipparchus.util.MathUtils;
30
31 /**
32 * Class that models a circle.
33 * The parameters of problem are:
34 * <ul>
35 * <li>the x-coordinate of the circle center,</li>
36 * <li>the y-coordinate of the circle center,</li>
37 * <li>the radius of the circle.</li>
38 * </ul>
39 * The model functions are:
40 * <ul>
41 * <li>for each triplet (cx, cy, r), the (x, y) coordinates of a point on the
42 * corresponding circle.</li>
43 * </ul>
44 */
45 class CircleProblem {
46 /** Cloud of points assumed to be fitted by a circle. */
47 private final ArrayList<double[]> points;
48 /** Error on the x-coordinate of the points. */
49 private final double xSigma;
50 /** Error on the y-coordinate of the points. */
51 private final double ySigma;
52 /** Number of points on the circumference (when searching which
53 model point is closest to a given "observation". */
54 private final int resolution;
55
56 /**
57 * @param xError Assumed error for the x-coordinate of the circle points.
58 * @param yError Assumed error for the y-coordinate of the circle points.
59 * @param searchResolution Number of points to try when searching the one
60 * that is closest to a given "observed" point.
61 */
62 public CircleProblem(double xError,
63 double yError,
64 int searchResolution) {
65 points = new ArrayList<double[]>();
66 xSigma = xError;
67 ySigma = yError;
68 resolution = searchResolution;
69 }
70
71 /**
72 * @param xError Assumed error for the x-coordinate of the circle points.
73 * @param yError Assumed error for the y-coordinate of the circle points.
74 */
75 public CircleProblem(double xError,
76 double yError) {
77 this(xError, yError, 500);
78 }
79
80 public void addPoint(double px, double py) {
81 points.add(new double[] { px, py });
82 }
83
84 public double[] target() {
85 final double[] t = new double[points.size() * 2];
86 for (int i = 0; i < points.size(); i++) {
87 final double[] p = points.get(i);
88 final int index = i * 2;
89 t[index] = p[0];
90 t[index + 1] = p[1];
91 }
92
93 return t;
94 }
95
96 public double[] weight() {
97 final double wX = 1 / (xSigma * xSigma);
98 final double wY = 1 / (ySigma * ySigma);
99 final double[] w = new double[points.size() * 2];
100 for (int i = 0; i < points.size(); i++) {
101 final int index = i * 2;
102 w[index] = wX;
103 w[index + 1] = wY;
104 }
105
106 return w;
107 }
108
109 public MultivariateVectorFunction getModelFunction() {
110 return new MultivariateVectorFunction() {
111 public double[] value(double[] params) {
112 final double cx = params[0];
113 final double cy = params[1];
114 final double r = params[2];
115
116 final double[] model = new double[points.size() * 2];
117
118 final double deltaTheta = MathUtils.TWO_PI / resolution;
119 for (int i = 0; i < points.size(); i++) {
120 final double[] p = points.get(i);
121 final double px = p[0];
122 final double py = p[1];
123
124 double bestX = 0;
125 double bestY = 0;
126 double dMin = Double.POSITIVE_INFINITY;
127
128 // Find the angle for which the circle passes closest to the
129 // current point (using a resolution of 100 points along the
130 // circumference).
131 for (double theta = 0; theta <= MathUtils.TWO_PI; theta += deltaTheta) {
132 final double currentX = cx + r * FastMath.cos(theta);
133 final double currentY = cy + r * FastMath.sin(theta);
134 final double dX = currentX - px;
135 final double dY = currentY - py;
136 final double d = dX * dX + dY * dY;
137 if (d < dMin) {
138 dMin = d;
139 bestX = currentX;
140 bestY = currentY;
141 }
142 }
143
144 final int index = i * 2;
145 model[index] = bestX;
146 model[index + 1] = bestY;
147 }
148
149 return model;
150 }
151 };
152 }
153
154 public MultivariateMatrixFunction getModelFunctionJacobian() {
155 return new MultivariateMatrixFunction() {
156 public double[][] value(double[] point) {
157 return jacobian(point);
158 }
159 };
160 }
161
162 private double[][] jacobian(double[] params) {
163 final double[][] jacobian = new double[points.size() * 2][3];
164
165 for (int i = 0; i < points.size(); i++) {
166 final int index = i * 2;
167 // Partial derivative wrt x-coordinate of center.
168 jacobian[index][0] = 1;
169 jacobian[index + 1][0] = 0;
170 // Partial derivative wrt y-coordinate of center.
171 jacobian[index][1] = 0;
172 jacobian[index + 1][1] = 1;
173 // Partial derivative wrt radius.
174 final double[] p = points.get(i);
175 jacobian[index][2] = (p[0] - params[0]) / params[2];
176 jacobian[index + 1][2] = (p[1] - params[1]) / params[2];
177 }
178
179 return jacobian;
180 }
181 }