View Javadoc
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 }