1 /*
2 * Copyright (C) 2015 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.estimators;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.geometry.AffineTransformation2D;
20 import com.irurueta.geometry.CoincidentLinesException;
21 import com.irurueta.geometry.Line2D;
22 import com.irurueta.numerical.robust.MSACRobustEstimator;
23 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
24 import com.irurueta.numerical.robust.RobustEstimator;
25 import com.irurueta.numerical.robust.RobustEstimatorException;
26 import com.irurueta.numerical.robust.RobustEstimatorMethod;
27
28 import java.util.List;
29
30 /**
31 * Finds the best affine 2D transformation for provided collections of matched
32 * 2D lines using MSAC algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class MSACLineCorrespondenceAffineTransformation2DRobustEstimator
36 extends LineCorrespondenceAffineTransformation2DRobustEstimator {
37
38 /**
39 * Constant defining default threshold to determine whether lines are
40 * inliers or not.
41 * Residuals to determine whether lines are inliers or not are computed by
42 * comparing two lines algebraically (e.g. doing the dot product of their
43 * parameters).
44 * A residual of 0 indicates that dot product was 1 or -1 and lines were
45 * equal.
46 * A residual of 1 indicates that dot product was 0 and lines were
47 * orthogonal.
48 * If dot product between lines is -1, then although their director vectors
49 * are opposed, lines are considered equal, since sign changes are not taken
50 * into account and their residuals will be 0.
51 */
52 public static final double DEFAULT_THRESHOLD = 1e-6;
53
54 /**
55 * Minimum value that can be set as threshold.
56 * Threshold must be strictly greater than 0.0.
57 */
58 public static final double MIN_THRESHOLD = 0.0;
59
60 /**
61 * Threshold to determine whether lines are inliers or not when testing
62 * possible estimation solutions.
63 * The threshold refers to the amount of error (i.e. distance and director
64 * vector angle difference) a possible solution has on a matched pair of
65 * lines.
66 */
67 private double threshold;
68
69 /**
70 * Constructor.
71 */
72 public MSACLineCorrespondenceAffineTransformation2DRobustEstimator() {
73 super();
74 threshold = DEFAULT_THRESHOLD;
75 }
76
77 /**
78 * Constructor with lists of lines to be used to estimate an affine 2D
79 * transformation.
80 * Lines in the list located at the same position are considered to be
81 * matched. Hence, both lists must have the same size, and their size must
82 * be greater or equal than MINIMUM_SIZE.
83 *
84 * @param inputLines list of input lines to be used to estimate an affine
85 * 2D transformation.
86 * @param outputLines list of output lines to be used to estimate an affine
87 * 2D transformation.
88 * @throws IllegalArgumentException if provided lists of lines don't have
89 * the same size or their size is smaller than MINIMUM_SIZE.
90 */
91 public MSACLineCorrespondenceAffineTransformation2DRobustEstimator(
92 final List<Line2D> inputLines, final List<Line2D> outputLines) {
93 super(inputLines, outputLines);
94 threshold = DEFAULT_THRESHOLD;
95 }
96
97 /**
98 * Constructor.
99 *
100 * @param listener listener to be notified of events such as when estimation
101 * starts, ends or its progress significantly changes.
102 */
103 public MSACLineCorrespondenceAffineTransformation2DRobustEstimator(
104 final AffineTransformation2DRobustEstimatorListener listener) {
105 super(listener);
106 threshold = DEFAULT_THRESHOLD;
107 }
108
109 /**
110 * Constructor with listener and lists of lines to be used to estimate an
111 * affine 2D transformation.
112 * Lines in the list located at the same position are considered to be
113 * matched. Hence, both lists must have the same size, and their size must
114 * be greater or equal than MINIMUM_SIZE.
115 *
116 * @param listener listener to be notified of events such as when estimation
117 * starts, ends or its progress significantly changes.
118 * @param inputLines list of input lines to be used to estimate an affine
119 * 2D transformation.
120 * @param outputLines list of output lines to be used to estimate an affine
121 * 2D transformation.
122 * @throws IllegalArgumentException if provided lists of lines don't have
123 * the same size or their size is smaller than MINIMUM_SIZE.
124 */
125 public MSACLineCorrespondenceAffineTransformation2DRobustEstimator(
126 final AffineTransformation2DRobustEstimatorListener listener,
127 final List<Line2D> inputLines, final List<Line2D> outputLines) {
128 super(listener, inputLines, outputLines);
129 threshold = DEFAULT_THRESHOLD;
130 }
131
132 /**
133 * Returns threshold to determine whether lines are inliers or not when
134 * testing possible estimation solutions.
135 * Residuals to determine whether lines are inliers or not are computed by
136 * comparing two lines algebraically (e.g. doing the dot product of their
137 * parameters).
138 * A residual of 0 indicates that dot product was 1 or -1 and lines were
139 * equal.
140 * A residual of 1 indicates that dot product was 0 and lines were
141 * orthogonal.
142 * If dot product between lines is -1, then although their director vectors
143 * are opposed, lines are considered equal, since sign changes are not taken
144 * into account and their residuals will be 0.
145 *
146 * @return threshold to determine whether matched lines are inliers or not.
147 */
148 public double getThreshold() {
149 return threshold;
150 }
151
152 /**
153 * Sets threshold to determine whether lines are inliers or not when
154 * testing possible estimation solutions.
155 * Residuals to determine whether lines are inliers or not are computed by
156 * comparing two lines algebraically (e.g. doing the dot product of their
157 * parameters).
158 * A residual of 0 indicates that dot product was 1 or -1 and lines were
159 * equal.
160 * A residual of 1 indicates that dot product was 0 and lines were
161 * orthogonal.
162 * If dot product between lines is -1, then although their director vectors
163 * are opposed, lines are considered equal, since sign changes are not taken
164 * into account and their residuals will be 0.
165 *
166 * @param threshold threshold to determine whether matched lines are inliers
167 * or not.
168 * @throws IllegalArgumentException if provided value is equal or less than
169 * zero.
170 * @throws LockedException if robust estimator is locked because an
171 * estimation is already in progress.
172 */
173 public void setThreshold(final double threshold) throws LockedException {
174 if (isLocked()) {
175 throw new LockedException();
176 }
177 if (threshold <= MIN_THRESHOLD) {
178 throw new IllegalArgumentException();
179 }
180 this.threshold = threshold;
181 }
182
183 /**
184 * Estimates an affine 2D transformation using a robust estimator and
185 * the best set of matched 2D lines correspondences found using the robust
186 * estimator.
187 *
188 * @return an affine 2D transformation.
189 * @throws LockedException if robust estimator is locked because an
190 * estimation is already in progress.
191 * @throws NotReadyException if provided input data is not enough to start
192 * the estimation.
193 * @throws RobustEstimatorException if estimation fails for any reason
194 * (i.e. numerical instability, no solution available, etc).
195 */
196 @Override
197 public AffineTransformation2D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
198 if (isLocked()) {
199 throw new LockedException();
200 }
201 if (!isReady()) {
202 throw new NotReadyException();
203 }
204
205 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<AffineTransformation2D>() {
206
207 // line to be reused when computing residuals
208 private final Line2D testLine = new Line2D();
209
210 @Override
211 public double getThreshold() {
212 return threshold;
213 }
214
215 @Override
216 public int getTotalSamples() {
217 return inputLines.size();
218 }
219
220 @Override
221 public int getSubsetSize() {
222 return AffineTransformation2DRobustEstimator.MINIMUM_SIZE;
223 }
224
225 @Override
226 public void estimatePreliminarSolutions(
227 final int[] samplesIndices, final List<AffineTransformation2D> solutions) {
228 final var inputLine1 = inputLines.get(samplesIndices[0]);
229 final var inputLine2 = inputLines.get(samplesIndices[1]);
230 final var inputLine3 = inputLines.get(samplesIndices[2]);
231
232 final var outputLine1 = outputLines.get(samplesIndices[0]);
233 final var outputLine2 = outputLines.get(samplesIndices[1]);
234 final var outputLine3 = outputLines.get(samplesIndices[2]);
235
236 try {
237 final var transformation = new AffineTransformation2D(inputLine1, inputLine2, inputLine3,
238 outputLine1, outputLine2, outputLine3);
239 solutions.add(transformation);
240 } catch (final CoincidentLinesException e) {
241 // if lines are coincident, no solution is added
242 }
243 }
244
245 @Override
246 public double computeResidual(final AffineTransformation2D currentEstimation, final int i) {
247 final var inputLine = inputLines.get(i);
248 final var outputLine = outputLines.get(i);
249
250 // transform input line and store result in mTestLine
251 try {
252 currentEstimation.transform(inputLine, testLine);
253
254 return getResidual(outputLine, testLine);
255 } catch (final AlgebraException e) {
256 // this happens when internal matrix of affine transformation
257 // cannot be reverse (i.e. transformation is not well-defined,
258 // numerical instabilities, etc.)
259 return Double.MAX_VALUE;
260 }
261 }
262
263 @Override
264 public boolean isReady() {
265 return MSACLineCorrespondenceAffineTransformation2DRobustEstimator.this.isReady();
266 }
267
268 @Override
269 public void onEstimateStart(final RobustEstimator<AffineTransformation2D> estimator) {
270 if (mListener != null) {
271 mListener.onEstimateStart(
272 MSACLineCorrespondenceAffineTransformation2DRobustEstimator.this);
273 }
274 }
275
276 @Override
277 public void onEstimateEnd(final RobustEstimator<AffineTransformation2D> estimator) {
278 if (mListener != null) {
279 mListener.onEstimateEnd(MSACLineCorrespondenceAffineTransformation2DRobustEstimator.this);
280 }
281 }
282
283 @Override
284 public void onEstimateNextIteration(
285 final RobustEstimator<AffineTransformation2D> estimator, int iteration) {
286 if (mListener != null) {
287 mListener.onEstimateNextIteration(
288 MSACLineCorrespondenceAffineTransformation2DRobustEstimator.this, iteration);
289 }
290 }
291
292 @Override
293 public void onEstimateProgressChange(
294 final RobustEstimator<AffineTransformation2D> estimator, float progress) {
295 if (mListener != null) {
296 mListener.onEstimateProgressChange(
297 MSACLineCorrespondenceAffineTransformation2DRobustEstimator.this, progress);
298 }
299 }
300 });
301
302 try {
303 locked = true;
304 inliersData = null;
305 innerEstimator.setConfidence(confidence);
306 innerEstimator.setMaxIterations(maxIterations);
307 innerEstimator.setProgressDelta(progressDelta);
308 final var transformation = innerEstimator.estimate();
309 inliersData = innerEstimator.getInliersData();
310 return attemptRefine(transformation);
311 } catch (final com.irurueta.numerical.LockedException e) {
312 throw new LockedException(e);
313 } catch (final com.irurueta.numerical.NotReadyException e) {
314 throw new NotReadyException(e);
315 } finally {
316 locked = false;
317 }
318 }
319
320 /**
321 * Returns method being used for robust estimation.
322 *
323 * @return method being used for robust estimation.
324 */
325 @Override
326 public RobustEstimatorMethod getMethod() {
327 return RobustEstimatorMethod.MSAC;
328 }
329
330 /**
331 * Gets standard deviation used for Levenberg-Marquardt fitting during
332 * refinement.
333 * Returned value gives an indication of how much variance each residual
334 * has.
335 * Typically, this value is related to the threshold used on each robust
336 * estimation, since residuals of found inliers are within the range of
337 * such threshold.
338 *
339 * @return standard deviation used for refinement.
340 */
341 @Override
342 protected double getRefinementStandardDeviation() {
343 return threshold;
344 }
345 }