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.geometry.CoincidentPointsException;
19 import com.irurueta.geometry.CoordinatesType;
20 import com.irurueta.geometry.Point3D;
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 points using LMedS algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator
36 extends PointCorrespondenceProjectiveTransformation3DRobustEstimator {
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 = 1.0;
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 * Constructor.
81 */
82 public LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator() {
83 super();
84 stopThreshold = DEFAULT_STOP_THRESHOLD;
85 }
86
87 /**
88 * Constructor with lists of points to be used to estimate a projective 3D
89 * transformation.
90 * Points 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 inputPoints list of input points to be used to estimate a
95 * projective 3D transformation.
96 * @param outputPoints list of output points to be used to estimate a
97 * projective 3D transformation.
98 * @throws IllegalArgumentException if provided lists of points don't have
99 * the same size or their size is smaller than MINIMUM_SIZE.
100 */
101 public LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator(
102 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
103 super(inputPoints, outputPoints);
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 LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator(
114 final ProjectiveTransformation3DRobustEstimatorListener listener) {
115 super(listener);
116 stopThreshold = DEFAULT_STOP_THRESHOLD;
117 }
118
119 /**
120 * Constructor with listener and lists of points to be used to estimate a
121 * projective 3D transformation.
122 * Points 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 * stars, ends or its progress significantly changes.
128 * @param inputPoints list of input points to be used to estimate a
129 * projective 3D transformation.
130 * @param outputPoints list of output points to be used to estimate a
131 * projective 3D transformation.
132 * @throws IllegalArgumentException if provided lists of points don't have
133 * the same size or their size is smaller than MINIMUM_SIZE.
134 */
135 public LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator(
136 final ProjectiveTransformation3DRobustEstimatorListener listener,
137 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
138 super(listener, inputPoints, outputPoints);
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 a projective 3D transformation using a robust estimator and
200 * the best set of matched 3D point correspondences found using the robust
201 * estimator.
202 *
203 * @return a projective 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 @Override
212 public ProjectiveTransformation3D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
213 if (isLocked()) {
214 throw new LockedException();
215 }
216 if (!isReady()) {
217 throw new NotReadyException();
218 }
219
220 final var innerEstimator = new LMedSRobustEstimator<>(
221 new LMedSRobustEstimatorListener<ProjectiveTransformation3D>() {
222
223 // point to be reused when computing residuals
224 private final Point3D testPoint = Point3D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
225
226 @Override
227 public int getTotalSamples() {
228 return inputPoints.size();
229 }
230
231 @Override
232 public int getSubsetSize() {
233 return ProjectiveTransformation3DRobustEstimator.MINIMUM_SIZE;
234 }
235
236 @Override
237 public void estimatePreliminarSolutions(
238 final int[] samplesIndices, final List<ProjectiveTransformation3D> solutions) {
239 final var inputPoint1 = inputPoints.get(samplesIndices[0]);
240 final var inputPoint2 = inputPoints.get(samplesIndices[1]);
241 final var inputPoint3 = inputPoints.get(samplesIndices[2]);
242 final var inputPoint4 = inputPoints.get(samplesIndices[3]);
243 final var inputPoint5 = inputPoints.get(samplesIndices[4]);
244
245 final var outputPoint1 = outputPoints.get(samplesIndices[0]);
246 final var outputPoint2 = outputPoints.get(samplesIndices[1]);
247 final var outputPoint3 = outputPoints.get(samplesIndices[2]);
248 final var outputPoint4 = outputPoints.get(samplesIndices[3]);
249 final var outputPoint5 = outputPoints.get(samplesIndices[4]);
250
251 try {
252 final var transformation = new ProjectiveTransformation3D(inputPoint1, inputPoint2,
253 inputPoint3, inputPoint4, inputPoint5, outputPoint1, outputPoint2, outputPoint3,
254 outputPoint4, outputPoint5);
255 solutions.add(transformation);
256 } catch (final CoincidentPointsException e) {
257 // if points are coincident, no solution is added
258 }
259 }
260
261 @Override
262 public double computeResidual(final ProjectiveTransformation3D currentEstimation, final int i) {
263 final var inputPoint = inputPoints.get(i);
264 final var outputPoint = outputPoints.get(i);
265
266 // transform input point and store result in mTestPoint
267 currentEstimation.transform(inputPoint, testPoint);
268
269 return outputPoint.distanceTo(testPoint);
270 }
271
272 @Override
273 public boolean isReady() {
274 return LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator.this.isReady();
275 }
276
277 @Override
278 public void onEstimateStart(final RobustEstimator<ProjectiveTransformation3D> estimator) {
279 if (listener != null) {
280 listener.onEstimateStart(
281 LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator.this);
282 }
283 }
284
285 @Override
286 public void onEstimateEnd(final RobustEstimator<ProjectiveTransformation3D> estimator) {
287 if (listener != null) {
288 listener.onEstimateEnd(
289 LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator.this);
290 }
291 }
292
293 @Override
294 public void onEstimateNextIteration(
295 final RobustEstimator<ProjectiveTransformation3D> estimator, final int iteration) {
296 if (listener != null) {
297 listener.onEstimateNextIteration(
298 LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator.this,
299 iteration);
300 }
301 }
302
303 @Override
304 public void onEstimateProgressChange(
305 final RobustEstimator<ProjectiveTransformation3D> estimator, final float progress) {
306 if (listener != null) {
307 listener.onEstimateProgressChange(
308 LMedSPointCorrespondenceProjectiveTransformation3DRobustEstimator.this,
309 progress);
310 }
311 }
312 });
313
314 try {
315 locked = true;
316 inliersData = null;
317 innerEstimator.setConfidence(confidence);
318 innerEstimator.setMaxIterations(maxIterations);
319 innerEstimator.setProgressDelta(progressDelta);
320 innerEstimator.setStopThreshold(stopThreshold);
321 final var transformation = innerEstimator.estimate();
322 inliersData = innerEstimator.getInliersData();
323 return attemptRefine(transformation);
324 } catch (final com.irurueta.numerical.LockedException e) {
325 throw new LockedException(e);
326 } catch (final com.irurueta.numerical.NotReadyException e) {
327 throw new NotReadyException(e);
328 } finally {
329 locked = false;
330 }
331 }
332
333 /**
334 * Returns method being used for robust estimation.
335 *
336 * @return method being used for robust estimation.
337 */
338 @Override
339 public RobustEstimatorMethod getMethod() {
340 return RobustEstimatorMethod.LMEDS;
341 }
342
343 /**
344 * Gets standard deviation used for Levenberg-Marquardt fitting during
345 * refinement.
346 * Returned value gives an indication of how much variance each residual
347 * has.
348 * Typically, this value is related to the threshold used on each robust
349 * estimation, since residuals of found inliers are within the range of
350 * such threshold.
351 *
352 * @return standard deviation used for refinement.
353 */
354 @Override
355 protected double getRefinementStandardDeviation() {
356 final var inliersData = (LMedSRobustEstimator.LMedSInliersData) getInliersData();
357
358 // avoid setting a threshold too strict
359 final var threshold = inliersData.getEstimatedThreshold();
360 return Math.max(threshold, stopThreshold);
361 }
362 }