1 /*
2 * Copyright (C) 2017 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.geometry.refiners;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.geometry.InhomogeneousPoint2D;
20 import com.irurueta.geometry.Line2D;
21 import com.irurueta.geometry.estimators.LockedException;
22 import com.irurueta.geometry.estimators.NotReadyException;
23 import com.irurueta.numerical.EvaluationException;
24 import com.irurueta.numerical.GradientEstimator;
25 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
26 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
27 import com.irurueta.numerical.robust.InliersData;
28
29 import java.util.BitSet;
30 import java.util.List;
31
32 /**
33 * Refines an inhomogeneous 2D point by taking into account an initial
34 * estimation, inlier samples and their residuals.
35 * This class can be used to find a solution that minimizes error of inliers in
36 * LMSE terms.
37 * Typically, a refiner is used by a robust estimator, however it can also be
38 * useful in some other situations.
39 */
40 @SuppressWarnings("DuplicatedCode")
41 public class InhomogeneousPoint2DRefiner extends Point2DRefiner<InhomogeneousPoint2D> {
42
43 /**
44 * Constructor.
45 */
46 public InhomogeneousPoint2DRefiner() {
47 }
48
49 /**
50 * Constructor.
51 *
52 * @param initialEstimation initial estimation to be set.
53 * @param keepCovariance true if covariance of estimation must be kept after
54 * refinement, false otherwise.
55 * @param inliers set indicating which of the provided matches are inliers.
56 * @param residuals residuals for matched samples.
57 * @param numInliers number of inliers on initial estimation.
58 * @param samples collection of samples.
59 * @param refinementStandardDeviation standard deviation used for
60 * Levenberg-Marquardt fitting.
61 */
62 public InhomogeneousPoint2DRefiner(
63 final InhomogeneousPoint2D initialEstimation, final boolean keepCovariance, final BitSet inliers,
64 final double[] residuals, final int numInliers, final List<Line2D> samples,
65 final double refinementStandardDeviation) {
66 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples, refinementStandardDeviation);
67 }
68
69 /**
70 * Constructor.
71 *
72 * @param initialEstimation initial estimation to be set.
73 * @param keepCovariance true if covariance of estimation must be kept after
74 * refinement, false otherwise.
75 * @param inliersData inlier data, typically obtained from a robust
76 * estimator.
77 * @param samples collection of samples.
78 * @param refinementStandardDeviation standard deviation used for
79 * Levenberg-Marquardt fitting.
80 */
81 public InhomogeneousPoint2DRefiner(
82 final InhomogeneousPoint2D initialEstimation, final boolean keepCovariance, final InliersData inliersData,
83 final List<Line2D> samples, final double refinementStandardDeviation) {
84 super(initialEstimation, keepCovariance, inliersData, samples, refinementStandardDeviation);
85 }
86
87 /**
88 * Refines provided initial estimation.
89 *
90 * @return refined estimation.
91 * @throws NotReadyException if not enough input data has been provided.
92 * @throws LockedException if estimator is locked because refinement is
93 * already in progress.
94 * @throws RefinerException if refinement fails for some reason (e.g. unable
95 * to converge to a result).
96 */
97 @Override
98 public InhomogeneousPoint2D refine() throws NotReadyException, LockedException, RefinerException {
99 final var result = new InhomogeneousPoint2D();
100 refine(result);
101 return result;
102 }
103
104 /**
105 * Refines provided initial estimation.
106 * This method always sets a value into provided result instance regardless
107 * of the fact that error has actually improved in LMSE terms or not.
108 *
109 * @param result instance where refined estimation will be stored.
110 * @return true if result improved (decreases) in LMSE terms respect to
111 * initial estimation, false if no improvement has been achieved.
112 * @throws NotReadyException if not enough input data has been provided.
113 * @throws LockedException if estimator is locked because refinement is
114 * already in progress.
115 * @throws RefinerException if refinement fails for some reason (e.g. unable
116 * to converge to a result).
117 */
118 @Override
119 public boolean refine(final InhomogeneousPoint2D result) throws NotReadyException, LockedException,
120 RefinerException {
121 if (isLocked()) {
122 throw new LockedException();
123 }
124 if (!isReady()) {
125 throw new NotReadyException();
126 }
127
128 locked = true;
129
130 if (listener != null) {
131 listener.onRefineStart(this, initialEstimation);
132 }
133
134 final var initialTotalResidual = totalResidual(initialEstimation);
135
136 try {
137 final var initParams = initialEstimation.asArray();
138
139 // output values to be fitted/optimized will contain residuals
140 final var y = new double[numInliers];
141 // input values will contain lines to compute residuals
142 final var nDims = Line2D.LINE_NUMBER_PARAMS;
143 final var x = new Matrix(numInliers, nDims);
144 final var nSamples = inliers.length();
145 var pos = 0;
146 for (var i = 0; i < nSamples; i++) {
147 if (inliers.get(i)) {
148 // sample is inlier
149 final var line = samples.get(i);
150 line.normalize();
151 x.setElementAt(pos, 0, line.getA());
152 x.setElementAt(pos, 1, line.getB());
153 x.setElementAt(pos, 2, line.getC());
154
155 y[pos] = residuals[i];
156 pos++;
157 }
158 }
159
160 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
161
162 private final Line2D line = new Line2D();
163
164 private final InhomogeneousPoint2D point = new InhomogeneousPoint2D();
165
166 private final GradientEstimator gradientEstimator = new GradientEstimator(p -> {
167 this.point.setCoordinates(p);
168 return residual(this.point, line);
169 });
170
171 @Override
172 public int getNumberOfDimensions() {
173 return nDims;
174 }
175
176 @Override
177 public double[] createInitialParametersArray() {
178 return initParams;
179 }
180
181 @Override
182 public double evaluate(final int i, final double[] point, final double[] params,
183 final double[] derivatives) throws EvaluationException {
184 // point contains, a,b,c values for line
185 line.setParameters(point);
186
187 // param contains coordinates of point
188 this.point.setCoordinates(params);
189
190 final var y = residual(this.point, line);
191 gradientEstimator.gradient(params, derivatives);
192
193 return y;
194 }
195 };
196
197 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
198 getRefinementStandardDeviation());
199
200 fitter.fit();
201
202 // obtain estimated params
203 final var params = fitter.getA();
204
205 // update point
206 result.setCoordinates(params);
207
208 if (keepCovariance) {
209 // keep covariance
210 covariance = fitter.getCovar();
211 }
212
213 final var finalTotalResidual = totalResidual(result);
214 final var errorDecreased = finalTotalResidual < initialTotalResidual;
215
216 if (listener != null) {
217 listener.onRefineEnd(this, initialEstimation, result, errorDecreased);
218 }
219
220 return errorDecreased;
221 } catch (final Exception e) {
222 throw new RefinerException(e);
223 } finally {
224 locked = false;
225 }
226 }
227 }