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.CoordinatesType;
20 import com.irurueta.geometry.EuclideanTransformation2D;
21 import com.irurueta.geometry.MetricTransformation2D;
22 import com.irurueta.geometry.Point2D;
23 import com.irurueta.geometry.estimators.LockedException;
24 import com.irurueta.geometry.estimators.NotReadyException;
25 import com.irurueta.numerical.EvaluationException;
26 import com.irurueta.numerical.GradientEstimator;
27 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFitter;
28 import com.irurueta.numerical.fitting.LevenbergMarquardtMultiDimensionFunctionEvaluator;
29 import com.irurueta.numerical.robust.InliersData;
30
31 import java.util.BitSet;
32 import java.util.List;
33
34 /**
35 * Refine a 2D metric transformation by taking into account an initial
36 * estimation, inlier point matches and their residuals.
37 * This class can be used to find a solution that minimizes error of inliers in
38 * LMSE terms.
39 * Typically, a refiner is used by a robust estimator, however it can also be
40 * useful in some other situations.
41 */
42 @SuppressWarnings("DuplicatedCode")
43 public class MetricTransformation2DRefiner extends
44 PairMatchesAndInliersDataRefiner<MetricTransformation2D, Point2D, Point2D> {
45
46 /**
47 * Point to be reused when computing residuals.
48 */
49 private final Point2D residualTestPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
50
51 /**
52 * Standard deviation used for Levenberg-Marquardt fitting during
53 * refinement.
54 * Returned value gives an indication of how much variance each residual
55 * has.
56 * Typically, this value is related to the threshold used on each robust
57 * estimation, since residuals of found inliers are within the range of
58 * such threshold.
59 */
60 private double refinementStandardDeviation;
61
62 /**
63 * Constructor.
64 */
65 public MetricTransformation2DRefiner() {
66 }
67
68 /**
69 * Constructor.
70 *
71 * @param initialEstimation initial estimation to be set.
72 * @param keepCovariance true if covariance of estimation must be kept after
73 * refinement, false otherwise.
74 * @param inliers set indicating which of the provided matches are inliers.
75 * @param residuals residuals for matched samples.
76 * @param numInliers number of inliers on initial estimation.
77 * @param samples1 1st set of paired samples.
78 * @param samples2 2nd set of paired samples.
79 * @param refinementStandardDeviation standard deviation used for
80 * Levenberg-Marquardt fitting.
81 */
82 public MetricTransformation2DRefiner(
83 final MetricTransformation2D initialEstimation, final boolean keepCovariance,
84 final BitSet inliers, final double[] residuals, final int numInliers,
85 final List<Point2D> samples1, final List<Point2D> samples2, final double refinementStandardDeviation) {
86 super(initialEstimation, keepCovariance, inliers, residuals, numInliers, samples1, samples2);
87 this.refinementStandardDeviation = refinementStandardDeviation;
88 }
89
90 /**
91 * Constructor.
92 *
93 * @param initialEstimation initial estimation to be set.
94 * @param keepCovariance true if covariance of estimation must be kept after
95 * refinement, false otherwise.
96 * @param inliersData inlier data, typically obtained from a robust
97 * estimator.
98 * @param samples1 1st set of paired samples.
99 * @param samples2 2nd set of paired samples.
100 * @param refinementStandardDeviation standard deviation used for
101 * Levenberg-Marquardt fitting.
102 */
103 public MetricTransformation2DRefiner(
104 final MetricTransformation2D initialEstimation, final boolean keepCovariance,
105 final InliersData inliersData, final List<Point2D> samples1, final List<Point2D> samples2,
106 final double refinementStandardDeviation) {
107 super(initialEstimation, keepCovariance, inliersData, samples1, samples2);
108 this.refinementStandardDeviation = refinementStandardDeviation;
109 }
110
111 /**
112 * Gets standard deviation used for Levenberg-Marquardt fitting during
113 * refinement.
114 * Returned value gives an indication of how much variance each residual
115 * has.
116 * Typically, this value is related to the threshold used on each robust
117 * estimation, since residuals of found inliers are within the range of
118 * such threshold.
119 *
120 * @return standard deviation used for refinement.
121 */
122 public double getRefinementStandardDeviation() {
123 return refinementStandardDeviation;
124 }
125
126 /**
127 * Sets standard deviation used for Levenberg-Marquardt fitting during
128 * refinement.
129 * Returned value gives an indication of how much variance each residual
130 * has.
131 * Typically, this value is related to the threshold used on each robust
132 * estimation, since residuals of found inliers are within the range of such
133 * threshold.
134 *
135 * @param refinementStandardDeviation standard deviation used for
136 * refinement.
137 * @throws LockedException if estimator is locked.
138 */
139 public void setRefinementStandardDeviation(final double refinementStandardDeviation) throws LockedException {
140 if (isLocked()) {
141 throw new LockedException();
142 }
143 this.refinementStandardDeviation = refinementStandardDeviation;
144 }
145
146 /**
147 * Refines provided initial estimation.
148 *
149 * @return refines estimation.
150 * @throws NotReadyException if not enough input data has been provided.
151 * @throws LockedException if estimator is locked because refinement is
152 * already in progress.
153 * @throws RefinerException if refinement fails for some reason (e.g. unable
154 * to converge to a result).
155 */
156 @Override
157 public MetricTransformation2D refine() throws NotReadyException, LockedException, RefinerException {
158 final var result = new MetricTransformation2D();
159 refine(result);
160 return result;
161 }
162
163 /**
164 * Refines provided initial estimation.
165 * This method always sets a value into provided result instance regardless
166 * of the fact that error has actually improved in LMSE terms or not.
167 *
168 * @param result instance where refined estimation will be stored.
169 * @return true if result improves (error decreases) in LMSE terms respect
170 * to initial estimation, false if no improvement has been achieved.
171 * @throws NotReadyException if not enough input data has been provided.
172 * @throws LockedException if estimator is locked because refinement is
173 * already in progress.
174 * @throws RefinerException if refinement fails for some reason (e.g. unable
175 * to converge to a result).
176 */
177 @Override
178 public boolean refine(final MetricTransformation2D result) throws NotReadyException, LockedException,
179 RefinerException {
180 if (isLocked()) {
181 throw new LockedException();
182 }
183 if (!isReady()) {
184 throw new NotReadyException();
185 }
186
187 locked = true;
188
189 if (listener != null) {
190 listener.onRefineStart(this, initialEstimation);
191 }
192
193 final var initialTotalResidual = totalResidual(initialEstimation);
194
195 try {
196 // parameters: rotation angle + scale + translation
197 final var initParams = new double[2 + EuclideanTransformation2D.NUM_TRANSLATION_COORDS];
198 // copy values
199 initParams[0] = initialEstimation.getScale();
200 initParams[1] = initialEstimation.getRotation().getTheta();
201 System.arraycopy(initialEstimation.getTranslation(), 0, initParams, 2,
202 EuclideanTransformation2D.NUM_TRANSLATION_COORDS);
203
204 // output values to be fitted/optimized will contain residuals
205 final var y = new double[numInliers];
206 // input values will contain 2 sets of 2D points to compute residuals
207 final var nDims = 2 * Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH;
208 final var x = new Matrix(numInliers, nDims);
209 final var nSamples = inliers.length();
210 var pos = 0;
211 for (var i = 0; i < nSamples; i++) {
212 if (inliers.get(i)) {
213 // sample is inlier
214 final var inputPoint = samples1.get(i);
215 final var outputPoint = samples2.get(i);
216 inputPoint.normalize();
217 outputPoint.normalize();
218 x.setElementAt(pos, 0, inputPoint.getHomX());
219 x.setElementAt(pos, 1, inputPoint.getHomY());
220 x.setElementAt(pos, 2, inputPoint.getHomW());
221 x.setElementAt(pos, 3, outputPoint.getHomX());
222 x.setElementAt(pos, 4, outputPoint.getHomY());
223 x.setElementAt(pos, 5, outputPoint.getHomW());
224
225 y[pos] = residuals[i];
226 pos++;
227 }
228 }
229
230 final var evaluator = new LevenbergMarquardtMultiDimensionFunctionEvaluator() {
231
232 private final Point2D inputPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
233
234 private final Point2D outputPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
235
236 private final MetricTransformation2D transformation = new MetricTransformation2D();
237
238 private final GradientEstimator gradientEstimator = new GradientEstimator(params -> {
239 // copy values
240 transformation.setScale(params[0]);
241 transformation.getRotation().setTheta(params[1]);
242 System.arraycopy(params, 2, transformation.getTranslation(), 0,
243 EuclideanTransformation2D.NUM_TRANSLATION_COORDS);
244
245 return residual(transformation, inputPoint, outputPoint);
246 });
247
248 @Override
249 public int getNumberOfDimensions() {
250 return nDims;
251 }
252
253 @Override
254 public double[] createInitialParametersArray() {
255 return initParams;
256 }
257
258 @Override
259 public double evaluate(final int i, final double[] point, final double[] params,
260 final double[] derivatives) throws EvaluationException {
261 inputPoint.setHomogeneousCoordinates(point[0], point[1], point[2]);
262 outputPoint.setHomogeneousCoordinates(point[3], point[4], point[5]);
263
264 // copy values
265 transformation.setScale(params[0]);
266 transformation.getRotation().setTheta(params[1]);
267 System.arraycopy(params, 2, transformation.getTranslation(), 0,
268 EuclideanTransformation2D.NUM_TRANSLATION_COORDS);
269
270 final var y = residual(transformation, inputPoint, outputPoint);
271 gradientEstimator.gradient(params, derivatives);
272
273 return y;
274 }
275 };
276
277 final var fitter = new LevenbergMarquardtMultiDimensionFitter(evaluator, x, y,
278 getRefinementStandardDeviation());
279
280 fitter.fit();
281
282 // obtain estimated params
283 final var params = fitter.getA();
284
285 // update transformation
286 result.setScale(params[0]);
287 result.getRotation().setTheta(params[1]);
288 System.arraycopy(params, 2, result.getTranslation(), 0,
289 EuclideanTransformation2D.NUM_TRANSLATION_COORDS);
290
291 if (keepCovariance) {
292 // keep covariance
293 covariance = fitter.getCovar();
294 }
295
296 final var finalTotalResidual = totalResidual(result);
297 final var errorDecreased = finalTotalResidual < initialTotalResidual;
298
299 if (listener != null) {
300 listener.onRefineEnd(this, initialEstimation, result, errorDecreased);
301 }
302
303 return errorDecreased;
304
305 } catch (final Exception e) {
306 throw new RefinerException(e);
307 } finally {
308 locked = false;
309 }
310 }
311
312 /**
313 * Computes the residual between the Euclidean transformation and a pair or
314 * matched points.
315 *
316 * @param transformation a transformation.
317 * @param inputPoint input 2D point.
318 * @param outputPoint output 2D point.
319 * @return residual.
320 */
321 private double residual(final MetricTransformation2D transformation, final Point2D inputPoint,
322 final Point2D outputPoint) {
323 inputPoint.normalize();
324 outputPoint.normalize();
325
326 transformation.transform(inputPoint, residualTestPoint);
327 return residualTestPoint.distanceTo(outputPoint);
328 }
329
330 /**
331 * Computes total residual among all provided inlier samples.
332 *
333 * @param transformation a transformation.
334 * @return total residual.
335 */
336 private double totalResidual(final MetricTransformation2D transformation) {
337 var result = 0.0;
338
339 final var nSamples = inliers.length();
340 for (var i = 0; i < nSamples; i++) {
341 if (inliers.get(i)) {
342 // sample is inlier
343 final var inputPoint = samples1.get(i);
344 final var outputPoint = samples2.get(i);
345 inputPoint.normalize();
346 outputPoint.normalize();
347 result += residual(transformation, inputPoint, outputPoint);
348 }
349 }
350
351 return result;
352 }
353 }