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