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