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.AffineTransformation3D;
20 import com.irurueta.geometry.CoincidentPlanesException;
21 import com.irurueta.geometry.Plane;
22 import com.irurueta.numerical.robust.LMedSRobustEstimator;
23 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
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 3D transformation for provided collections of matched
32 * planes using LMedS algorithm.
33 */
34 public class LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator
35 extends PlaneCorrespondenceAffineTransformation3DRobustEstimator {
36
37 /**
38 * Default value to be used for stop threshold. Stop threshold can be used
39 * to keep the algorithm iterating in case that best estimated threshold
40 * using median of residuals is not small enough. Once a solution is found
41 * that generates a threshold below this value, the algorithm will stop.
42 * The stop threshold can be used to prevent the LMedS algorithm iterating
43 * too many times in cases where samples have a very similar accuracy.
44 * For instance, in cases where proportion of outliers is very small (close
45 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
46 * iterate for a long time trying to find the best solution when indeed
47 * there is no need to do that if a reasonable threshold has already been
48 * reached.
49 * Because of this behaviour the stop threshold can be set to a value much
50 * lower than the one typically used in RANSAC, and yet the algorithm could
51 * still produce even smaller thresholds in estimated results.
52 */
53 public static final double DEFAULT_STOP_THRESHOLD = 1e-6;
54
55 /**
56 * Minimum allowed stop threshold value.
57 */
58 public static final double MIN_STOP_THRESHOLD = 0.0;
59
60 /**
61 * Threshold to be used to keep the algorithm iterating in case that best
62 * estimated threshold using median of residuals is not small enough. Once
63 * a solution is found that generates a threshold below this value, the
64 * algorithm will stop.
65 * The stop threshold can be used to prevent the LMedS algorithm iterating
66 * too many times in cases where samples have a very similar accuracy.
67 * For instance, in cases where proportion of outliers is very small (close
68 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
69 * iterate for a long time trying to find the best solution when indeed
70 * there is no need to do that if a reasonable threshold has already been
71 * reached.
72 * Because of this behaviour the stop threshold can be set to a value much
73 * lower than the one typically used in RANSAC, and yet the algorithm could
74 * still produce even smaller thresholds in estimated results.
75 */
76 private double stopThreshold;
77
78
79 /**
80 * Constructor.
81 */
82 public LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator() {
83 super();
84 stopThreshold = DEFAULT_STOP_THRESHOLD;
85 }
86
87 /**
88 * Constructor with lists of planes to be used to estimate an affine 3D
89 * transformation.
90 * Planes in the list located at the same position are considered to be
91 * matched. Hence, both lists must have the same size, and their size must
92 * be greater or equal than MINIMUM_SIZE.
93 *
94 * @param inputPlanes list of input planes to be used to estimate an affine
95 * 3D transformation.
96 * @param outputPlanes list of output planes to be used to estimate an affine
97 * 3D transformation.
98 * @throws IllegalArgumentException if provided lists of lines don't have
99 * the same size or their size is smaller than MINIMUM_SIZE.
100 */
101 public LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
102 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
103 super(inputPlanes, outputPlanes);
104 stopThreshold = DEFAULT_STOP_THRESHOLD;
105 }
106
107 /**
108 * Constructor.
109 *
110 * @param listener listener to be notified of events such as when estimation
111 * starts, ends or its progress significantly changes.
112 */
113 public LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
114 final AffineTransformation3DRobustEstimatorListener listener) {
115 super(listener);
116 stopThreshold = DEFAULT_STOP_THRESHOLD;
117 }
118
119 /**
120 * Constructor with listener and lists of planes to be used to estimate an
121 * affine 3D transformation.
122 * Planes in the list located at the same position are considered to be
123 * matched. Hence, both lists must have the same size, and their size must
124 * be greater or equal than MINIMUM_SIZE.
125 *
126 * @param listener listener to be notified of events such as when estimation
127 * starts, ends or its progress significantly changes.
128 * @param inputPlanes list of input planes to be used to estimate an affine
129 * 3D transformation.
130 * @param outputPlanes list of output planes to be used to estimate an affine
131 * 3D transformation.
132 * @throws IllegalArgumentException if provided lists of planes don't have
133 * the same size or their size is smaller than MINIMUM_SIZE.
134 */
135 public LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator(
136 final AffineTransformation3DRobustEstimatorListener listener,
137 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
138 super(listener, inputPlanes, outputPlanes);
139 stopThreshold = DEFAULT_STOP_THRESHOLD;
140 }
141
142 /**
143 * Returns threshold to be used to keep the algorithm iterating in case that
144 * best estimated threshold using median of residuals is not small enough.
145 * Once a solution is found that generates a threshold below this value, the
146 * algorithm will stop.
147 * The stop threshold can be used to prevent the LMedS algorithm iterating
148 * too many times in cases where samples have a very similar accuracy.
149 * For instance, in cases where proportion of outliers is very small (close
150 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
151 * iterate for a long time trying to find the best solution when indeed
152 * there is no need to do that if a reasonable threshold has already been
153 * reached.
154 * Because of this behaviour the stop threshold can be set to a value much
155 * lower than the one typically used in RANSAC, and yet the algorithm could
156 * still produce even smaller thresholds in estimated results.
157 *
158 * @return stop threshold to stop the algorithm prematurely when a certain
159 * accuracy has been reached.
160 */
161 public double getStopThreshold() {
162 return stopThreshold;
163 }
164
165 /**
166 * Sets threshold to be used to keep the algorithm iterating in case that
167 * best estimated threshold using median of residuals is not small enough.
168 * Once a solution is found that generates a threshold below this value, the
169 * algorithm will stop.
170 * The stop threshold can be used to prevent the LMedS algorithm iterating
171 * too many times in cases where samples have a very similar accuracy.
172 * For instance, in cases where proportion of outliers is very small (close
173 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
174 * iterate for a long time trying to find the best solution when indeed
175 * there is no need to do that if a reasonable threshold has already been
176 * reached.
177 * Because of this behaviour the stop threshold can be set to a value much
178 * lower than the one typically used in RANSAC, and yet the algorithm could
179 * still produce even smaller thresholds in estimated results.
180 *
181 * @param stopThreshold stop threshold to stop the algorithm prematurely
182 * when a certain accuracy has been reached.
183 * @throws IllegalArgumentException if provided value is zero or negative.
184 * @throws LockedException if robust estimator is locked because an
185 * estimation is already in progress.
186 */
187 public void setStopThreshold(final double stopThreshold) throws LockedException {
188 if (isLocked()) {
189 throw new LockedException();
190 }
191 if (stopThreshold <= MIN_STOP_THRESHOLD) {
192 throw new IllegalArgumentException();
193 }
194
195 this.stopThreshold = stopThreshold;
196 }
197
198 /**
199 * Estimates an affine 3D transformation using a robust estimator and
200 * the best set of matched 3D lines correspondences found using the robust
201 * estimator.
202 *
203 * @return an affine 3D transformation.
204 * @throws LockedException if robust estimator is locked because an
205 * estimation is already in progress.
206 * @throws NotReadyException if provided input data is not enough to start
207 * the estimation.
208 * @throws RobustEstimatorException if estimation fails for any reason
209 * (i.e. numerical instability, no solution available, etc).
210 */
211 @SuppressWarnings("DuplicatedCode")
212 @Override
213 public AffineTransformation3D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
214 if (isLocked()) {
215 throw new LockedException();
216 }
217 if (!isReady()) {
218 throw new NotReadyException();
219 }
220
221 final var innerEstimator = new LMedSRobustEstimator<>(
222 new LMedSRobustEstimatorListener<AffineTransformation3D>() {
223
224 // plane to be reused when computing residuals
225 private final Plane testPlane = new Plane();
226
227 @Override
228 public int getTotalSamples() {
229 return inputPlanes.size();
230 }
231
232 @Override
233 public int getSubsetSize() {
234 return AffineTransformation3DRobustEstimator.MINIMUM_SIZE;
235 }
236
237 @Override
238 public void estimatePreliminarSolutions(
239 final int[] samplesIndices, final List<AffineTransformation3D> solutions) {
240 final var inputLine1 = inputPlanes.get(samplesIndices[0]);
241 final var inputLine2 = inputPlanes.get(samplesIndices[1]);
242 final var inputLine3 = inputPlanes.get(samplesIndices[2]);
243 final var inputLine4 = inputPlanes.get(samplesIndices[3]);
244
245 final var outputLine1 = outputPlanes.get(samplesIndices[0]);
246 final var outputLine2 = outputPlanes.get(samplesIndices[1]);
247 final var outputLine3 = outputPlanes.get(samplesIndices[2]);
248 final var outputLine4 = outputPlanes.get(samplesIndices[3]);
249
250 try {
251 final var transformation = new AffineTransformation3D(inputLine1, inputLine2, inputLine3,
252 inputLine4, outputLine1, outputLine2, outputLine3, outputLine4);
253 solutions.add(transformation);
254 } catch (final CoincidentPlanesException e) {
255 // if lines are coincident, no solution is added
256 }
257 }
258
259 @Override
260 public double computeResidual(final AffineTransformation3D currentEstimation, final int i) {
261 final var inputLine = inputPlanes.get(i);
262 final var outputLine = outputPlanes.get(i);
263
264 // transform input line and store result in mTestLine
265 try {
266 currentEstimation.transform(inputLine, testPlane);
267
268 return getResidual(outputLine, testPlane);
269 } catch (final AlgebraException e) {
270 // this happens when internal matrix of affine transformation
271 // cannot be reverse (i.e. transformation is not well-defined,
272 // numerical instabilities, etc.)
273 return Double.MAX_VALUE;
274 }
275 }
276
277 @Override
278 public boolean isReady() {
279 return LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator.this.isReady();
280 }
281
282 @Override
283 public void onEstimateStart(final RobustEstimator<AffineTransformation3D> estimator) {
284 if (listener != null) {
285 listener.onEstimateStart(
286 LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator.this);
287 }
288 }
289
290 @Override
291 public void onEstimateEnd(final RobustEstimator<AffineTransformation3D> estimator) {
292 if (listener != null) {
293 listener.onEstimateEnd(
294 LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator.this);
295 }
296 }
297
298 @Override
299 public void onEstimateNextIteration(
300 final RobustEstimator<AffineTransformation3D> estimator, final int iteration) {
301 if (listener != null) {
302 listener.onEstimateNextIteration(
303 LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator.this,
304 iteration);
305 }
306 }
307
308 @Override
309 public void onEstimateProgressChange(
310 final RobustEstimator<AffineTransformation3D> estimator, final float progress) {
311 if (listener != null) {
312 listener.onEstimateProgressChange(
313 LMedSPlaneCorrespondenceAffineTransformation3DRobustEstimator.this,
314 progress);
315 }
316 }
317 });
318
319 try {
320 locked = true;
321 inliersData = null;
322 innerEstimator.setConfidence(confidence);
323 innerEstimator.setMaxIterations(maxIterations);
324 innerEstimator.setProgressDelta(progressDelta);
325 innerEstimator.setStopThreshold(stopThreshold);
326 final var transformation = innerEstimator.estimate();
327 inliersData = innerEstimator.getInliersData();
328 return attemptRefine(transformation);
329 } catch (final com.irurueta.numerical.LockedException e) {
330 throw new LockedException(e);
331 } catch (final com.irurueta.numerical.NotReadyException e) {
332 throw new NotReadyException(e);
333 } finally {
334 locked = false;
335 }
336 }
337
338 /**
339 * Returns method being used for robust estimation.
340 *
341 * @return method being used for robust estimation.
342 */
343 @Override
344 public RobustEstimatorMethod getMethod() {
345 return RobustEstimatorMethod.LMEDS;
346 }
347
348 /**
349 * Gets standard deviation used for Levenberg-Marquardt fitting during
350 * refinement.
351 * Returned value gives an indication of how much variance each residual
352 * has.
353 * Typically, this value is related to the threshold used on each robust
354 * estimation, since residuals of found inliers are within the range of
355 * such threshold.
356 *
357 * @return standard deviation used for refinement.
358 */
359 @Override
360 protected double getRefinementStandardDeviation() {
361 final var inliersData = (LMedSRobustEstimator.LMedSInliersData) getInliersData();
362
363 // avoid setting a threshold too strict
364 final var threshold = inliersData.getEstimatedThreshold();
365 return Math.max(threshold, stopThreshold);
366 }
367 }