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.HomogeneousPoint2D;
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 homogeneous 2D point by taking into account an initial estimation,
34 * 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 HomogeneousPoint2DRefiner extends Point2DRefiner<HomogeneousPoint2D> {
42
43 /**
44 * Constructor.
45 */
46 public HomogeneousPoint2DRefiner() {
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 HomogeneousPoint2DRefiner(
63 final HomogeneousPoint2D 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 HomogeneousPoint2DRefiner(
82 final HomogeneousPoint2D 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 HomogeneousPoint2D refine() throws NotReadyException, LockedException, RefinerException {
99 final var result = new HomogeneousPoint2D();
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 HomogeneousPoint2D result) throws NotReadyException, LockedException, RefinerException {
120 if (isLocked()) {
121 throw new LockedException();
122 }
123 if (!isReady()) {
124 throw new NotReadyException();
125 }
126
127 locked = true;
128
129 if (listener != null) {
130 listener.onRefineStart(this, initialEstimation);
131 }
132
133 final var initialTotalResidual = totalResidual(initialEstimation);
134
135 try {
136 final var initParams = initialEstimation.asArray();
137
138 // output value to be fitted/optimized will contain residuals
139 final var y = new double[numInliers];
140 // input values will contain planes to compute residuals
141 final var nDims = Line2D.LINE_NUMBER_PARAMS;
142 final var x = new Matrix(numInliers, nDims);
143 final var nSamples = inliers.length();
144 var pos = 0;
145 for (var i = 0; i < nSamples; i++) {
146 if (inliers.get(i)) {
147 // sample is inlier
148 final var line = samples.get(i);
149 line.normalize();
150 x.setElementAt(pos, 0, line.getA());
151 x.setElementAt(pos, 1, line.getB());
152 x.setElementAt(pos, 2, line.getC());
153
154 y[pos] = residuals[i];
155 pos++;
156 }
157 }
158
159 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
160
161 private final Line2D line = new Line2D();
162
163 private final HomogeneousPoint2D point = new HomogeneousPoint2D();
164
165 private final GradientEstimator gradientEstimator = new GradientEstimator(p -> {
166 this.point.setCoordinates(p);
167 return residual(this.point, line);
168 });
169
170 @Override
171 public int getNumberOfDimensions() {
172 return nDims;
173 }
174
175 @Override
176 public double[] createInitialParametersArray() {
177 return initParams;
178 }
179
180 @Override
181 public double evaluate(final int i, final double[] point, final double[] params,
182 final double[] derivatives) throws EvaluationException {
183 // point contains a,b,c values for line
184 line.setParameters(point);
185
186 // params contains coordinates of point
187 this.point.setCoordinates(params);
188
189 final var y = residual(this.point, line);
190 gradientEstimator.gradient(params, derivatives);
191
192 return y;
193 }
194 };
195
196 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
197 getRefinementStandardDeviation());
198
199 fitter.fit();
200
201 // obtain estimated params
202 final var params = fitter.getA();
203
204 // update point
205 result.setCoordinates(params);
206
207 if (keepCovariance) {
208 // keep covariance
209 covariance = fitter.getCovar();
210 }
211
212 final var finalTotalResidual = totalResidual(result);
213 final var errorDecreased = finalTotalResidual < initialTotalResidual;
214
215 if (listener != null) {
216 listener.onRefineEnd(this, initialEstimation, result, errorDecreased);
217 }
218
219 return errorDecreased;
220 } catch (final Exception e) {
221 throw new RefinerException(e);
222 } finally {
223 locked = false;
224 }
225 }
226 }