1 /*
2 * @file
3 * This file contains implementation of
4 * com.irurueta.geometry.estimators.LMedSMetricTransformation3DRobustEstimator
5 *
6 * @author Alberto Irurueta (alberto@irurueta.com)
7 * @date March 24, 2017.
8 */
9 package com.irurueta.geometry.estimators;
10
11 import com.irurueta.geometry.CoordinatesType;
12 import com.irurueta.geometry.MetricTransformation3D;
13 import com.irurueta.geometry.Point3D;
14 import com.irurueta.numerical.robust.LMedSRobustEstimator;
15 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
16 import com.irurueta.numerical.robust.RobustEstimator;
17 import com.irurueta.numerical.robust.RobustEstimatorException;
18 import com.irurueta.numerical.robust.RobustEstimatorMethod;
19
20 import java.util.ArrayList;
21 import java.util.List;
22
23 /**
24 * Finds the best metric 3D transformation for provided collections of
25 * matched 3D points using LMedS algorithm.
26 */
27 public class LMedSMetricTransformation3DRobustEstimator extends MetricTransformation3DRobustEstimator {
28
29 /**
30 * Default value ot be used for stop threshold. Stop threshold can be used
31 * to keep the algorithm iterating in case that best estimated threshold
32 * using median of residuals is not small enough. Once a solution is found
33 * that generates a threshold below this value, the algorithm will stop.
34 * The stop threshold can be used to prevent the LMedS algorithm iterating
35 * too many times in cases where samples have a very similar accuracy.
36 * For instance, in cases where proportion of outliers is very small (close
37 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
38 * iterate for a long time trying to find the best solution when indeed
39 * there is no need to do that if a reasonable threshold has already been
40 * reached.
41 * Because of this behaviour the stop threshold can be set to a value much
42 * lower than the one typically used in RANSAC, and yet the algorithm could
43 * still produce even smaller thresholds in estimated results.
44 */
45 public static final double DEFAULT_STOP_THRESHOLD = 1.0;
46
47 /**
48 * Minimum allowed stop threshold value.
49 */
50 public static final double MIN_STOP_THRESHOLD = 0.0;
51
52 /**
53 * Threshold to be used to keep the algorithm iterating in case that best
54 * estimated threshold using median of residuals is not small enough. Once
55 * a solution is found that generates a threshold below this value, the
56 * algorithm will stop.
57 * The stop threshold can be used to prevent the LMedS algorithm iterating
58 * too many times in cases where samples have a very similar accuracy.
59 * For instance, in cases where proportion of outliers is very small (close
60 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
61 * iterate for a long time trying to find the best solution when indeed
62 * there is no need to do that if a reasonable threshold has already been
63 * reached.
64 * Because of this behaviour the stop threshold can be set to a value much
65 * lower than the one typically used in RANSAC, and yet the algorithm could
66 * still produce even smaller thresholds in estimated results.
67 */
68 private double stopThreshold;
69
70 /**
71 * Constructor.
72 */
73 public LMedSMetricTransformation3DRobustEstimator() {
74 super();
75 stopThreshold = DEFAULT_STOP_THRESHOLD;
76 }
77
78 /**
79 * Constructor with lists of points to be used to estimate a metric 3D
80 * transformation.
81 * Points in the list located at the same position are considered to be
82 * matched. Hence, both lists must have the same size, and their size must
83 * be greater or equal than MINIMUM_SIZE.
84 *
85 * @param inputPoints list of input points to be used to estimate a
86 * metric 3D transformation.
87 * @param outputPoints list of output points to be used to estimate a
88 * metric 3D transformation.
89 * @throws IllegalArgumentException if provided lists of points don't have
90 * the same size or their size is smaller than MINIMUM_SIZE.
91 */
92 public LMedSMetricTransformation3DRobustEstimator(
93 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
94 super(inputPoints, outputPoints);
95 stopThreshold = DEFAULT_STOP_THRESHOLD;
96 }
97
98 /**
99 * Constructor.
100 *
101 * @param listener listener to be notified of events such as when estimation
102 * starts, ends or its progress significantly changes.
103 */
104 public LMedSMetricTransformation3DRobustEstimator(final MetricTransformation3DRobustEstimatorListener listener) {
105 super(listener);
106 stopThreshold = DEFAULT_STOP_THRESHOLD;
107 }
108
109 /**
110 * Constructor with listener and lists of points to be used to estimate a
111 * metric 2D transformation.
112 * Points 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 inputPoints list of input points to be used to estimate a
119 * metric 3D transformation.
120 * @param outputPoints list of output points to be used to estimate a
121 * metric 3D transformation.
122 * @throws IllegalArgumentException if provided lists of points don't have
123 * the same size or their size is smaller than MINIMUM_SIZE.
124 */
125 public LMedSMetricTransformation3DRobustEstimator(
126 final MetricTransformation3DRobustEstimatorListener listener,
127 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
128 super(listener, inputPoints, outputPoints);
129 stopThreshold = DEFAULT_STOP_THRESHOLD;
130 }
131
132 /**
133 * Constructor.
134 *
135 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
136 */
137 public LMedSMetricTransformation3DRobustEstimator(final boolean weakMinimumSizeAllowed) {
138 super(weakMinimumSizeAllowed);
139 stopThreshold = DEFAULT_STOP_THRESHOLD;
140 }
141
142 /**
143 * Constructor with lists of points to be used to estimate a metric 3D
144 * transformation.
145 * Points in the list located at the same position are considered to be
146 * matched. Hence, both lists must have the same size, and their size must
147 * be greater or equal than MINIMUM_SIZE.
148 *
149 * @param inputPoints list of input points to be used to estimate a
150 * metric 3D transformation.
151 * @param outputPoints list of output points to be used to estimate a
152 * metric 3D transformation.
153 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
154 * @throws IllegalArgumentException if provided lists of points don't have
155 * the same size or their size is smaller than MINIMUM_SIZE.
156 */
157 public LMedSMetricTransformation3DRobustEstimator(
158 final List<Point3D> inputPoints, final List<Point3D> outputPoints, final boolean weakMinimumSizeAllowed) {
159 super(inputPoints, outputPoints, weakMinimumSizeAllowed);
160 stopThreshold = DEFAULT_STOP_THRESHOLD;
161 }
162
163 /**
164 * Constructor.
165 *
166 * @param listener listener to be notified of events such as when estimation
167 * starts, ends or its progress significantly changes.
168 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
169 */
170 public LMedSMetricTransformation3DRobustEstimator(
171 final MetricTransformation3DRobustEstimatorListener listener, final boolean weakMinimumSizeAllowed) {
172 super(listener, weakMinimumSizeAllowed);
173 stopThreshold = DEFAULT_STOP_THRESHOLD;
174 }
175
176 /**
177 * Constructor with listener and lists of points to be used to estimate a
178 * metric 2D transformation.
179 * Points in the list located at the same position are considered to be
180 * matched. Hence, both lists must have the same size, and their size must
181 * be greater or equal than MINIMUM_SIZE.
182 *
183 * @param listener listener to be notified of events such as when estimation
184 * starts, ends or its progress significantly changes.
185 * @param inputPoints list of input points to be used to estimate a
186 * metric 3D transformation.
187 * @param outputPoints list of output points to be used to estimate a
188 * metric 3D transformation.
189 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
190 * @throws IllegalArgumentException if provided lists of points don't have
191 * the same size or their size is smaller than MINIMUM_SIZE.
192 */
193 public LMedSMetricTransformation3DRobustEstimator(
194 final MetricTransformation3DRobustEstimatorListener listener,
195 final List<Point3D> inputPoints, List<Point3D> outputPoints, final boolean weakMinimumSizeAllowed) {
196 super(listener, inputPoints, outputPoints, weakMinimumSizeAllowed);
197 stopThreshold = DEFAULT_STOP_THRESHOLD;
198 }
199
200 /**
201 * Returns threshold to be used to keep the algorithm iterating in case that
202 * best estimated threshold using median of residuals is not small enough.
203 * Once a solution is found that generates a threshold below this value, the
204 * algorithm will stop.
205 * The stop threshold can be used to prevent the LMedS algorithm iterating
206 * too many times in cases where samples have a very similar accuracy.
207 * For instance, in cases where proportion of outliers is very small (close
208 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
209 * iterate for a long time trying to find the best solution when indeed
210 * there is no need to do that if a reasonable threshold has already been
211 * reached.
212 * Because of this behaviour the stop threshold can be set to a value much
213 * lower than the one typically used in RANSAC, and yet the algorithm could
214 * still produce even smaller thresholds in estimated results.
215 *
216 * @return stop threshold to stop the algorithm prematurely when a certain
217 * accuracy has been reached.
218 */
219 public double getStopThreshold() {
220 return stopThreshold;
221 }
222
223 /**
224 * Sets threshold to be used to keep the algorithm iterating in case that
225 * best estimated threshold using median of residuals is not small enough.
226 * Once a solution is found that generates a threshold below this value, the
227 * algorithm will stop.
228 * The stop threshold can be used to prevent the LMedS algorithm iterating
229 * too many times in cases where samples have a very similar accuracy.
230 * For instance, in cases where proportion of outliers is very small (close
231 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
232 * iterate for a long time trying to find the best solution when indeed
233 * there is no need to do that if a reasonable threshold has already been
234 * reached.
235 * Because of this behaviour the stop threshold can be set to a value much
236 * lower than the one typically used in RANSAC, and yet the algorithm could
237 * still produce even smaller thresholds in estimated results
238 *
239 * @param stopThreshold stop threshold to stop the algorithm prematurely
240 * when a certain accuracy has been reached
241 * @throws IllegalArgumentException if provided value is zero or negative
242 * @throws LockedException if robust estimator is locked because an
243 * estimation is already in progress
244 */
245 public void setStopThreshold(final double stopThreshold) throws LockedException {
246 if (isLocked()) {
247 throw new LockedException();
248 }
249 if (stopThreshold <= MIN_STOP_THRESHOLD) {
250 throw new IllegalArgumentException();
251 }
252
253 this.stopThreshold = stopThreshold;
254 }
255
256 /**
257 * Estimates a metric 3D transformation using a robust estimator and
258 * the best set of matched 3D point correspondences found using the robust
259 * estimator.
260 *
261 * @return a metric 3D transformation.
262 * @throws LockedException if robust estimator is locked because an
263 * estimation is already in progress.
264 * @throws NotReadyException if provided input data is not enough to start
265 * the estimation.
266 * @throws RobustEstimatorException if estimation fails for any reason
267 * (i.e. numerical instability, no solution available, etc).
268 */
269 @Override
270 public MetricTransformation3D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
271 if (isLocked()) {
272 throw new LockedException();
273 }
274 if (!isReady()) {
275 throw new NotReadyException();
276 }
277
278 final var innerEstimator = new LMedSRobustEstimator<>(
279 new LMedSRobustEstimatorListener<MetricTransformation3D>() {
280
281 // point to be reused when computing residuals
282 private final Point3D testPoint = Point3D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
283
284 private final MetricTransformation3DEstimator nonRobustEstimator =
285 new MetricTransformation3DEstimator(isWeakMinimumSizeAllowed());
286
287 private final List<Point3D> subsetInputPoints = new ArrayList<>();
288 private final List<Point3D> subsetOutputPoints = new ArrayList<>();
289
290 @Override
291 public int getTotalSamples() {
292 return inputPoints.size();
293 }
294
295 @Override
296 public int getSubsetSize() {
297 return nonRobustEstimator.getMinimumPoints();
298 }
299
300 @SuppressWarnings("DuplicatedCode")
301 @Override
302 public void estimatePreliminarSolutions(final int[] samplesIndices,
303 final List<MetricTransformation3D> solutions) {
304 subsetInputPoints.clear();
305 subsetOutputPoints.clear();
306 for (final var samplesIndex : samplesIndices) {
307 subsetInputPoints.add(inputPoints.get(samplesIndex));
308 subsetOutputPoints.add(outputPoints.get(
309 samplesIndex));
310 }
311
312 try {
313 nonRobustEstimator.setPoints(subsetInputPoints, subsetOutputPoints);
314 solutions.add(nonRobustEstimator.estimate());
315 } catch (final Exception e) {
316 // if points are coincident, no solution is added
317 }
318 }
319
320 @Override
321 public double computeResidual(final MetricTransformation3D currentEstimation, final int i) {
322 final var inputPoint = inputPoints.get(i);
323 final var outputPoint = outputPoints.get(i);
324
325 // transform input point and store result in mTestPoint
326 currentEstimation.transform(inputPoint, testPoint);
327
328 return outputPoint.distanceTo(testPoint);
329 }
330
331 @Override
332 public boolean isReady() {
333 return LMedSMetricTransformation3DRobustEstimator.this.isReady();
334 }
335
336 @Override
337 public void onEstimateStart(final RobustEstimator<MetricTransformation3D> estimator) {
338 if (listener != null) {
339 listener.onEstimateStart(LMedSMetricTransformation3DRobustEstimator.this);
340 }
341 }
342
343 @Override
344 public void onEstimateEnd(final RobustEstimator<MetricTransformation3D> estimator) {
345 if (listener != null) {
346 listener.onEstimateEnd(LMedSMetricTransformation3DRobustEstimator.this);
347 }
348 }
349
350 @Override
351 public void onEstimateNextIteration(
352 final RobustEstimator<MetricTransformation3D> estimator, final int iteration) {
353 if (listener != null) {
354 listener.onEstimateNextIteration(
355 LMedSMetricTransformation3DRobustEstimator.this, iteration);
356 }
357 }
358
359 @Override
360 public void onEstimateProgressChange(
361 final RobustEstimator<MetricTransformation3D> estimator, final float progress) {
362 if (listener != null) {
363 listener.onEstimateProgressChange(
364 LMedSMetricTransformation3DRobustEstimator.this, progress);
365 }
366 }
367 });
368
369 try {
370 locked = true;
371 inliersData = null;
372 innerEstimator.setConfidence(confidence);
373 innerEstimator.setMaxIterations(maxIterations);
374 innerEstimator.setProgressDelta(progressDelta);
375 innerEstimator.setStopThreshold(stopThreshold);
376 final var transformation = innerEstimator.estimate();
377 inliersData = innerEstimator.getInliersData();
378 return attemptRefine(transformation);
379 } catch (final com.irurueta.numerical.LockedException e) {
380 throw new LockedException(e);
381 } catch (final com.irurueta.numerical.NotReadyException e) {
382 throw new NotReadyException(e);
383 } finally {
384 locked = false;
385 }
386 }
387
388 /**
389 * Returns method being used for robust estimation.
390 *
391 * @return method being used for robust estimation.
392 */
393 @Override
394 public RobustEstimatorMethod getMethod() {
395 return RobustEstimatorMethod.LMEDS;
396 }
397
398 /**
399 * Gets standard deviation used for Levenberg-Marquardt fitting during
400 * refinement.
401 * Returned value gives an indication of how much variance each residual
402 * has.
403 * Typically, this value is related to the threshold used on each robust
404 * estimation, since residuals of found inliers are within the range of
405 * such threshold.
406 *
407 * @return standard deviation used for refinement.
408 */
409 @Override
410 protected double getRefinementStandardDeviation() {
411 final var inliersData = (LMedSRobustEstimator.LMedSInliersData) getInliersData();
412 return inliersData.getEstimatedThreshold();
413 }
414 }